1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package com.irurueta.navigation.inertial.calibration.accelerometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.navigation.frames.CoordinateTransformation;
23 import com.irurueta.navigation.frames.ECEFFrame;
24 import com.irurueta.navigation.frames.FrameType;
25 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
26 import com.irurueta.navigation.frames.NEDFrame;
27 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
28 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
29 import com.irurueta.navigation.inertial.BodyKinematics;
30 import com.irurueta.navigation.inertial.ECEFGravity;
31 import com.irurueta.navigation.inertial.ECEFPosition;
32 import com.irurueta.navigation.inertial.ECEFVelocity;
33 import com.irurueta.navigation.inertial.NEDPosition;
34 import com.irurueta.navigation.inertial.NEDVelocity;
35 import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
36 import com.irurueta.navigation.inertial.calibration.CalibrationException;
37 import com.irurueta.navigation.inertial.calibration.IMUErrors;
38 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
39 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
40 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
41 import com.irurueta.statistics.UniformRandomizer;
42 import com.irurueta.units.Acceleration;
43 import com.irurueta.units.AccelerationUnit;
44 import org.junit.Test;
45
46 import java.util.ArrayList;
47 import java.util.Collection;
48 import java.util.Collections;
49 import java.util.List;
50 import java.util.Random;
51
52 import static org.junit.Assert.*;
53
54 public class KnownBiasAndGravityNormAccelerometerCalibratorTest implements
55 KnownBiasAndGravityNormAccelerometerCalibratorListener {
56
57 private static final double TIME_INTERVAL_SECONDS = 0.02;
58
59 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
60 private static final double DEG_TO_RAD = 0.01745329252;
61
62 private static final double MIN_ANGLE_DEGREES = -180.0;
63 private static final double MAX_ANGLE_DEGREES = 180.0;
64
65 private static final double MIN_LATITUDE_DEGREES = -90.0;
66 private static final double MAX_LATITUDE_DEGREES = 90.0;
67 private static final double MIN_LONGITUDE_DEGREES = -180.0;
68 private static final double MAX_LONGITUDE_DEGREES = 180.0;
69 private static final double MIN_HEIGHT = -50.0;
70 private static final double MAX_HEIGHT = 50.0;
71
72 private static final int LARGE_MEASUREMENT_NUMBER = 70000;
73
74 private static final double ABSOLUTE_ERROR = 1e-8;
75 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
76
77 private static final int TIMES = 70;
78
79 private int mCalibrateStart;
80 private int mCalibrateEnd;
81
82 @Test
83 public void testConstructor1() throws WrongSizeException {
84 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
85 new KnownBiasAndGravityNormAccelerometerCalibrator();
86
87
88 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
89 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
90 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
91 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
92 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
93 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
94 final Acceleration bx2 = new Acceleration(0.0,
95 AccelerationUnit.FEET_PER_SQUARED_SECOND);
96 calibrator.getBiasXAsAcceleration(bx2);
97 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
98 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
99 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
100 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
101 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
102 final Acceleration by2 = new Acceleration(0.0,
103 AccelerationUnit.FEET_PER_SQUARED_SECOND);
104 calibrator.getBiasYAsAcceleration(by2);
105 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
106 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
107 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
108 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
109 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
110 final Acceleration bz2 = new Acceleration(0.0,
111 AccelerationUnit.FEET_PER_SQUARED_SECOND);
112 calibrator.getBiasZAsAcceleration(bz2);
113 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
114 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
115 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
116 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
117 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
118 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
119 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
120 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
121 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
122 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
123 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
124 final double[] bias1 = calibrator.getBias();
125 assertArrayEquals(bias1, new double[3], 0.0);
126 final double[] bias2 = new double[3];
127 calibrator.getBias(bias2);
128 assertArrayEquals(bias1, bias2, 0.0);
129 final Matrix b1 = calibrator.getBiasAsMatrix();
130 assertEquals(b1, new Matrix(3, 1));
131 final Matrix b2 = new Matrix(3, 1);
132 calibrator.getBiasAsMatrix(b2);
133 assertEquals(b1, b2);
134 final Matrix ma1 = calibrator.getInitialMa();
135 assertEquals(ma1, new Matrix(3, 3));
136 final Matrix ma2 = new Matrix(3, 3);
137 calibrator.getInitialMa(ma2);
138 assertEquals(ma1, ma2);
139 assertNull(calibrator.getMeasurements());
140 assertFalse(calibrator.isCommonAxisUsed());
141 assertNull(calibrator.getListener());
142 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
143 assertFalse(calibrator.isReady());
144 assertFalse(calibrator.isRunning());
145 assertNull(calibrator.getEstimatedMa());
146 assertNull(calibrator.getEstimatedSx());
147 assertNull(calibrator.getEstimatedSy());
148 assertNull(calibrator.getEstimatedSz());
149 assertNull(calibrator.getEstimatedMxy());
150 assertNull(calibrator.getEstimatedMxz());
151 assertNull(calibrator.getEstimatedMyx());
152 assertNull(calibrator.getEstimatedMyz());
153 assertNull(calibrator.getEstimatedMzx());
154 assertNull(calibrator.getEstimatedMzy());
155 assertNull(calibrator.getEstimatedCovariance());
156 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
157 assertNull(calibrator.getGroundTruthGravityNorm());
158 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
159 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
160 }
161
162 @Test
163 public void testConstructor2() throws WrongSizeException {
164 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
165 new KnownBiasAndGravityNormAccelerometerCalibrator(this);
166
167
168 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
169 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
170 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
171 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
172 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
173 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
174 final Acceleration bx2 = new Acceleration(0.0,
175 AccelerationUnit.FEET_PER_SQUARED_SECOND);
176 calibrator.getBiasXAsAcceleration(bx2);
177 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
178 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
179 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
180 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
181 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
182 final Acceleration by2 = new Acceleration(0.0,
183 AccelerationUnit.FEET_PER_SQUARED_SECOND);
184 calibrator.getBiasYAsAcceleration(by2);
185 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
186 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
187 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
188 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
189 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
190 final Acceleration bz2 = new Acceleration(0.0,
191 AccelerationUnit.FEET_PER_SQUARED_SECOND);
192 calibrator.getBiasZAsAcceleration(bz2);
193 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
194 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
195 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
196 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
197 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
198 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
199 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
200 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
201 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
202 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
203 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
204 final double[] bias1 = calibrator.getBias();
205 assertArrayEquals(bias1, new double[3], 0.0);
206 final double[] bias2 = new double[3];
207 calibrator.getBias(bias2);
208 assertArrayEquals(bias1, bias2, 0.0);
209 final Matrix b1 = calibrator.getBiasAsMatrix();
210 assertEquals(b1, new Matrix(3, 1));
211 final Matrix b2 = new Matrix(3, 1);
212 calibrator.getBiasAsMatrix(b2);
213 assertEquals(b1, b2);
214 final Matrix ma1 = calibrator.getInitialMa();
215 assertEquals(ma1, new Matrix(3, 3));
216 final Matrix ma2 = new Matrix(3, 3);
217 calibrator.getInitialMa(ma2);
218 assertEquals(ma1, ma2);
219 assertNull(calibrator.getMeasurements());
220 assertFalse(calibrator.isCommonAxisUsed());
221 assertSame(calibrator.getListener(), this);
222 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
223 assertFalse(calibrator.isReady());
224 assertFalse(calibrator.isRunning());
225 assertNull(calibrator.getEstimatedMa());
226 assertNull(calibrator.getEstimatedSx());
227 assertNull(calibrator.getEstimatedSy());
228 assertNull(calibrator.getEstimatedSz());
229 assertNull(calibrator.getEstimatedMxy());
230 assertNull(calibrator.getEstimatedMxz());
231 assertNull(calibrator.getEstimatedMyx());
232 assertNull(calibrator.getEstimatedMyz());
233 assertNull(calibrator.getEstimatedMzx());
234 assertNull(calibrator.getEstimatedMzy());
235 assertNull(calibrator.getEstimatedCovariance());
236 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
237 assertNull(calibrator.getGroundTruthGravityNorm());
238 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
239 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
240 }
241
242 @Test
243 public void testConstructor3() throws WrongSizeException {
244 final Collection<StandardDeviationBodyKinematics> measurements =
245 Collections.emptyList();
246 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
247 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements);
248
249
250 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
251 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
252 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
253 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
254 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
255 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
256 final Acceleration bx2 = new Acceleration(0.0,
257 AccelerationUnit.FEET_PER_SQUARED_SECOND);
258 calibrator.getBiasXAsAcceleration(bx2);
259 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
260 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
261 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
262 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
263 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
264 final Acceleration by2 = new Acceleration(0.0,
265 AccelerationUnit.FEET_PER_SQUARED_SECOND);
266 calibrator.getBiasYAsAcceleration(by2);
267 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
268 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
269 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
270 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
271 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
272 final Acceleration bz2 = new Acceleration(0.0,
273 AccelerationUnit.FEET_PER_SQUARED_SECOND);
274 calibrator.getBiasZAsAcceleration(bz2);
275 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
276 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
277 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
278 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
279 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
280 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
281 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
282 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
283 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
284 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
285 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
286 final double[] bias1 = calibrator.getBias();
287 assertArrayEquals(bias1, new double[3], 0.0);
288 final double[] bias2 = new double[3];
289 calibrator.getBias(bias2);
290 assertArrayEquals(bias1, bias2, 0.0);
291 final Matrix b1 = calibrator.getBiasAsMatrix();
292 assertEquals(b1, new Matrix(3, 1));
293 final Matrix b2 = new Matrix(3, 1);
294 calibrator.getBiasAsMatrix(b2);
295 assertEquals(b1, b2);
296 final Matrix ma1 = calibrator.getInitialMa();
297 assertEquals(ma1, new Matrix(3, 3));
298 final Matrix ma2 = new Matrix(3, 3);
299 calibrator.getInitialMa(ma2);
300 assertEquals(ma1, ma2);
301 assertSame(calibrator.getMeasurements(), measurements);
302 assertFalse(calibrator.isCommonAxisUsed());
303 assertNull(calibrator.getListener());
304 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
305 assertFalse(calibrator.isReady());
306 assertFalse(calibrator.isRunning());
307 assertNull(calibrator.getEstimatedMa());
308 assertNull(calibrator.getEstimatedSx());
309 assertNull(calibrator.getEstimatedSy());
310 assertNull(calibrator.getEstimatedSz());
311 assertNull(calibrator.getEstimatedMxy());
312 assertNull(calibrator.getEstimatedMxz());
313 assertNull(calibrator.getEstimatedMyx());
314 assertNull(calibrator.getEstimatedMyz());
315 assertNull(calibrator.getEstimatedMzx());
316 assertNull(calibrator.getEstimatedMzy());
317 assertNull(calibrator.getEstimatedCovariance());
318 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
319 assertNull(calibrator.getGroundTruthGravityNorm());
320 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
321 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
322 }
323
324 @Test
325 public void testConstructor4() throws WrongSizeException {
326 final Collection<StandardDeviationBodyKinematics> measurements =
327 Collections.emptyList();
328 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
329 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
330 this);
331
332
333 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
334 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
335 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
336 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
337 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
338 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
339 final Acceleration bx2 = new Acceleration(0.0,
340 AccelerationUnit.FEET_PER_SQUARED_SECOND);
341 calibrator.getBiasXAsAcceleration(bx2);
342 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
343 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
344 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
345 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
346 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
347 final Acceleration by2 = new Acceleration(0.0,
348 AccelerationUnit.FEET_PER_SQUARED_SECOND);
349 calibrator.getBiasYAsAcceleration(by2);
350 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
351 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
352 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
353 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
354 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
355 final Acceleration bz2 = new Acceleration(0.0,
356 AccelerationUnit.FEET_PER_SQUARED_SECOND);
357 calibrator.getBiasZAsAcceleration(bz2);
358 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
359 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
360 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
361 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
362 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
363 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
364 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
365 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
366 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
367 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
368 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
369 final double[] bias1 = calibrator.getBias();
370 assertArrayEquals(bias1, new double[3], 0.0);
371 final double[] bias2 = new double[3];
372 calibrator.getBias(bias2);
373 assertArrayEquals(bias1, bias2, 0.0);
374 final Matrix b1 = calibrator.getBiasAsMatrix();
375 assertEquals(b1, new Matrix(3, 1));
376 final Matrix b2 = new Matrix(3, 1);
377 calibrator.getBiasAsMatrix(b2);
378 assertEquals(b1, b2);
379 final Matrix ma1 = calibrator.getInitialMa();
380 assertEquals(ma1, new Matrix(3, 3));
381 final Matrix ma2 = new Matrix(3, 3);
382 calibrator.getInitialMa(ma2);
383 assertEquals(ma1, ma2);
384 assertSame(calibrator.getMeasurements(), measurements);
385 assertFalse(calibrator.isCommonAxisUsed());
386 assertSame(calibrator.getListener(), this);
387 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
388 assertFalse(calibrator.isReady());
389 assertFalse(calibrator.isRunning());
390 assertNull(calibrator.getEstimatedMa());
391 assertNull(calibrator.getEstimatedSx());
392 assertNull(calibrator.getEstimatedSy());
393 assertNull(calibrator.getEstimatedSz());
394 assertNull(calibrator.getEstimatedMxy());
395 assertNull(calibrator.getEstimatedMxz());
396 assertNull(calibrator.getEstimatedMyx());
397 assertNull(calibrator.getEstimatedMyz());
398 assertNull(calibrator.getEstimatedMzx());
399 assertNull(calibrator.getEstimatedMzy());
400 assertNull(calibrator.getEstimatedCovariance());
401 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
402 assertNull(calibrator.getGroundTruthGravityNorm());
403 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
404 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
405 }
406
407 @Test
408 public void testConstructor5() throws WrongSizeException {
409 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
410 new KnownBiasAndGravityNormAccelerometerCalibrator(true);
411
412
413 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
414 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
415 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
416 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
417 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
418 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
419 final Acceleration bx2 = new Acceleration(0.0,
420 AccelerationUnit.FEET_PER_SQUARED_SECOND);
421 calibrator.getBiasXAsAcceleration(bx2);
422 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
423 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
424 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
425 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
426 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
427 final Acceleration by2 = new Acceleration(0.0,
428 AccelerationUnit.FEET_PER_SQUARED_SECOND);
429 calibrator.getBiasYAsAcceleration(by2);
430 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
431 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
432 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
433 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
434 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
435 final Acceleration bz2 = new Acceleration(0.0,
436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
437 calibrator.getBiasZAsAcceleration(bz2);
438 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
439 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
440 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
441 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
442 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
443 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
444 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
445 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
446 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
447 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
448 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
449 final double[] bias1 = calibrator.getBias();
450 assertArrayEquals(bias1, new double[3], 0.0);
451 final double[] bias2 = new double[3];
452 calibrator.getBias(bias2);
453 assertArrayEquals(bias1, bias2, 0.0);
454 final Matrix b1 = calibrator.getBiasAsMatrix();
455 assertEquals(b1, new Matrix(3, 1));
456 final Matrix b2 = new Matrix(3, 1);
457 calibrator.getBiasAsMatrix(b2);
458 assertEquals(b1, b2);
459 final Matrix ma1 = calibrator.getInitialMa();
460 assertEquals(ma1, new Matrix(3, 3));
461 final Matrix ma2 = new Matrix(3, 3);
462 calibrator.getInitialMa(ma2);
463 assertEquals(ma1, ma2);
464 assertNull(calibrator.getMeasurements());
465 assertTrue(calibrator.isCommonAxisUsed());
466 assertNull(calibrator.getListener());
467 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
468 assertFalse(calibrator.isReady());
469 assertFalse(calibrator.isRunning());
470 assertNull(calibrator.getEstimatedMa());
471 assertNull(calibrator.getEstimatedSx());
472 assertNull(calibrator.getEstimatedSy());
473 assertNull(calibrator.getEstimatedSz());
474 assertNull(calibrator.getEstimatedMxy());
475 assertNull(calibrator.getEstimatedMxz());
476 assertNull(calibrator.getEstimatedMyx());
477 assertNull(calibrator.getEstimatedMyz());
478 assertNull(calibrator.getEstimatedMzx());
479 assertNull(calibrator.getEstimatedMzy());
480 assertNull(calibrator.getEstimatedCovariance());
481 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
482 assertNull(calibrator.getGroundTruthGravityNorm());
483 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
484 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
485 }
486
487 @Test
488 public void testConstructor6() throws WrongSizeException {
489 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
490 new KnownBiasAndGravityNormAccelerometerCalibrator(
491 true, this);
492
493
494 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
495 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
496 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
497 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
498 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
499 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
500 final Acceleration bx2 = new Acceleration(0.0,
501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
502 calibrator.getBiasXAsAcceleration(bx2);
503 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
504 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
505 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
506 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
507 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
508 final Acceleration by2 = new Acceleration(0.0,
509 AccelerationUnit.FEET_PER_SQUARED_SECOND);
510 calibrator.getBiasYAsAcceleration(by2);
511 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
512 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
513 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
514 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
515 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
516 final Acceleration bz2 = new Acceleration(0.0,
517 AccelerationUnit.FEET_PER_SQUARED_SECOND);
518 calibrator.getBiasZAsAcceleration(bz2);
519 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
520 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
521 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
522 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
523 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
524 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
525 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
526 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
527 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
528 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
529 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
530 final double[] bias1 = calibrator.getBias();
531 assertArrayEquals(bias1, new double[3], 0.0);
532 final double[] bias2 = new double[3];
533 calibrator.getBias(bias2);
534 assertArrayEquals(bias1, bias2, 0.0);
535 final Matrix b1 = calibrator.getBiasAsMatrix();
536 assertEquals(b1, new Matrix(3, 1));
537 final Matrix b2 = new Matrix(3, 1);
538 calibrator.getBiasAsMatrix(b2);
539 assertEquals(b1, b2);
540 final Matrix ma1 = calibrator.getInitialMa();
541 assertEquals(ma1, new Matrix(3, 3));
542 final Matrix ma2 = new Matrix(3, 3);
543 calibrator.getInitialMa(ma2);
544 assertEquals(ma1, ma2);
545 assertNull(calibrator.getMeasurements());
546 assertTrue(calibrator.isCommonAxisUsed());
547 assertSame(calibrator.getListener(), this);
548 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
549 assertFalse(calibrator.isReady());
550 assertFalse(calibrator.isRunning());
551 assertNull(calibrator.getEstimatedMa());
552 assertNull(calibrator.getEstimatedSx());
553 assertNull(calibrator.getEstimatedSy());
554 assertNull(calibrator.getEstimatedSz());
555 assertNull(calibrator.getEstimatedMxy());
556 assertNull(calibrator.getEstimatedMxz());
557 assertNull(calibrator.getEstimatedMyx());
558 assertNull(calibrator.getEstimatedMyz());
559 assertNull(calibrator.getEstimatedMzx());
560 assertNull(calibrator.getEstimatedMzy());
561 assertNull(calibrator.getEstimatedCovariance());
562 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
563 assertNull(calibrator.getGroundTruthGravityNorm());
564 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
565 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
566 }
567
568 @Test
569 public void testConstructor7() throws WrongSizeException {
570 final Collection<StandardDeviationBodyKinematics> measurements =
571 Collections.emptyList();
572 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
573 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
574 true);
575
576
577 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
578 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
579 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
580 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
581 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
582 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
583 final Acceleration bx2 = new Acceleration(0.0,
584 AccelerationUnit.FEET_PER_SQUARED_SECOND);
585 calibrator.getBiasXAsAcceleration(bx2);
586 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
587 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
588 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
589 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
590 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
591 final Acceleration by2 = new Acceleration(0.0,
592 AccelerationUnit.FEET_PER_SQUARED_SECOND);
593 calibrator.getBiasYAsAcceleration(by2);
594 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
595 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
596 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
597 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
598 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
599 final Acceleration bz2 = new Acceleration(0.0,
600 AccelerationUnit.FEET_PER_SQUARED_SECOND);
601 calibrator.getBiasZAsAcceleration(bz2);
602 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
603 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
604 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
605 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
606 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
607 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
608 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
609 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
610 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
611 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
612 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
613 final double[] bias1 = calibrator.getBias();
614 assertArrayEquals(bias1, new double[3], 0.0);
615 final double[] bias2 = new double[3];
616 calibrator.getBias(bias2);
617 assertArrayEquals(bias1, bias2, 0.0);
618 final Matrix b1 = calibrator.getBiasAsMatrix();
619 assertEquals(b1, new Matrix(3, 1));
620 final Matrix b2 = new Matrix(3, 1);
621 calibrator.getBiasAsMatrix(b2);
622 assertEquals(b1, b2);
623 final Matrix ma1 = calibrator.getInitialMa();
624 assertEquals(ma1, new Matrix(3, 3));
625 final Matrix ma2 = new Matrix(3, 3);
626 calibrator.getInitialMa(ma2);
627 assertEquals(ma1, ma2);
628 assertSame(calibrator.getMeasurements(), measurements);
629 assertTrue(calibrator.isCommonAxisUsed());
630 assertNull(calibrator.getListener());
631 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
632 assertFalse(calibrator.isReady());
633 assertFalse(calibrator.isRunning());
634 assertNull(calibrator.getEstimatedMa());
635 assertNull(calibrator.getEstimatedSx());
636 assertNull(calibrator.getEstimatedSy());
637 assertNull(calibrator.getEstimatedSz());
638 assertNull(calibrator.getEstimatedMxy());
639 assertNull(calibrator.getEstimatedMxz());
640 assertNull(calibrator.getEstimatedMyx());
641 assertNull(calibrator.getEstimatedMyz());
642 assertNull(calibrator.getEstimatedMzx());
643 assertNull(calibrator.getEstimatedMzy());
644 assertNull(calibrator.getEstimatedCovariance());
645 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
646 assertNull(calibrator.getGroundTruthGravityNorm());
647 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
648 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
649 }
650
651 @Test
652 public void testConstructor8() throws WrongSizeException {
653 final Collection<StandardDeviationBodyKinematics> measurements =
654 Collections.emptyList();
655 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
656 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
657 true, this);
658
659
660 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
661 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
662 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
663 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
664 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
665 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
666 final Acceleration bx2 = new Acceleration(0.0,
667 AccelerationUnit.FEET_PER_SQUARED_SECOND);
668 calibrator.getBiasXAsAcceleration(bx2);
669 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
670 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
671 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
672 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
673 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
674 final Acceleration by2 = new Acceleration(0.0,
675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
676 calibrator.getBiasYAsAcceleration(by2);
677 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
678 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
679 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
680 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
681 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
682 final Acceleration bz2 = new Acceleration(0.0,
683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
684 calibrator.getBiasZAsAcceleration(bz2);
685 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
686 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
687 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
688 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
689 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
690 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
691 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
692 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
693 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
694 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
695 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
696 final double[] bias1 = calibrator.getBias();
697 assertArrayEquals(bias1, new double[3], 0.0);
698 final double[] bias2 = new double[3];
699 calibrator.getBias(bias2);
700 assertArrayEquals(bias1, bias2, 0.0);
701 final Matrix b1 = calibrator.getBiasAsMatrix();
702 assertEquals(b1, new Matrix(3, 1));
703 final Matrix b2 = new Matrix(3, 1);
704 calibrator.getBiasAsMatrix(b2);
705 assertEquals(b1, b2);
706 final Matrix ma1 = calibrator.getInitialMa();
707 assertEquals(ma1, new Matrix(3, 3));
708 final Matrix ma2 = new Matrix(3, 3);
709 calibrator.getInitialMa(ma2);
710 assertEquals(ma1, ma2);
711 assertSame(calibrator.getMeasurements(), measurements);
712 assertTrue(calibrator.isCommonAxisUsed());
713 assertSame(calibrator.getListener(), this);
714 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
715 assertFalse(calibrator.isReady());
716 assertFalse(calibrator.isRunning());
717 assertNull(calibrator.getEstimatedMa());
718 assertNull(calibrator.getEstimatedSx());
719 assertNull(calibrator.getEstimatedSy());
720 assertNull(calibrator.getEstimatedSz());
721 assertNull(calibrator.getEstimatedMxy());
722 assertNull(calibrator.getEstimatedMxz());
723 assertNull(calibrator.getEstimatedMyx());
724 assertNull(calibrator.getEstimatedMyz());
725 assertNull(calibrator.getEstimatedMzx());
726 assertNull(calibrator.getEstimatedMzy());
727 assertNull(calibrator.getEstimatedCovariance());
728 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
729 assertNull(calibrator.getGroundTruthGravityNorm());
730 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
731 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
732 }
733
734 @Test
735 public void testConstructor9() throws WrongSizeException {
736 final Matrix ba = generateBa();
737 final double biasX = ba.getElementAtIndex(0);
738 final double biasY = ba.getElementAtIndex(1);
739 final double biasZ = ba.getElementAtIndex(2);
740
741 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
742 new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
743
744
745 assertEquals(calibrator.getBiasX(), biasX, 0.0);
746 assertEquals(calibrator.getBiasY(), biasY, 0.0);
747 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
748 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
749 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
750 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
751 final Acceleration bx2 = new Acceleration(0.0,
752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
753 calibrator.getBiasXAsAcceleration(bx2);
754 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
755 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
756 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
757 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
758 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
759 final Acceleration by2 = new Acceleration(0.0,
760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
761 calibrator.getBiasYAsAcceleration(by2);
762 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
763 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
765 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
766 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
767 final Acceleration bz2 = new Acceleration(0.0,
768 AccelerationUnit.FEET_PER_SQUARED_SECOND);
769 calibrator.getBiasZAsAcceleration(bz2);
770 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
771 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
772 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
773 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
774 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
775 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
776 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
777 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
778 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
779 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
780 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
781 final double[] bias1 = calibrator.getBias();
782 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
783 final double[] bias2 = new double[3];
784 calibrator.getBias(bias2);
785 assertArrayEquals(bias1, bias2, 0.0);
786 final Matrix b1 = calibrator.getBiasAsMatrix();
787 assertEquals(b1, ba);
788 final Matrix b2 = new Matrix(3, 1);
789 calibrator.getBiasAsMatrix(b2);
790 assertEquals(b1, b2);
791 final Matrix ma1 = calibrator.getInitialMa();
792 assertEquals(ma1, new Matrix(3, 3));
793 final Matrix ma2 = new Matrix(3, 3);
794 calibrator.getInitialMa(ma2);
795 assertEquals(ma1, ma2);
796 assertNull(calibrator.getMeasurements());
797 assertFalse(calibrator.isCommonAxisUsed());
798 assertNull(calibrator.getListener());
799 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
800 assertFalse(calibrator.isReady());
801 assertFalse(calibrator.isRunning());
802 assertNull(calibrator.getEstimatedMa());
803 assertNull(calibrator.getEstimatedSx());
804 assertNull(calibrator.getEstimatedSy());
805 assertNull(calibrator.getEstimatedSz());
806 assertNull(calibrator.getEstimatedMxy());
807 assertNull(calibrator.getEstimatedMxz());
808 assertNull(calibrator.getEstimatedMyx());
809 assertNull(calibrator.getEstimatedMyz());
810 assertNull(calibrator.getEstimatedMzx());
811 assertNull(calibrator.getEstimatedMzy());
812 assertNull(calibrator.getEstimatedCovariance());
813 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
814 assertNull(calibrator.getGroundTruthGravityNorm());
815 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
816 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
817 }
818
819 @Test
820 public void testConstructor10() throws WrongSizeException {
821 final Matrix ba = generateBa();
822 final double biasX = ba.getElementAtIndex(0);
823 final double biasY = ba.getElementAtIndex(1);
824 final double biasZ = ba.getElementAtIndex(2);
825
826 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
827 new KnownBiasAndGravityNormAccelerometerCalibrator(
828 biasX, biasY, biasZ, this);
829
830
831 assertEquals(calibrator.getBiasX(), biasX, 0.0);
832 assertEquals(calibrator.getBiasY(), biasY, 0.0);
833 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
834 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
835 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
836 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
837 final Acceleration bx2 = new Acceleration(0.0,
838 AccelerationUnit.FEET_PER_SQUARED_SECOND);
839 calibrator.getBiasXAsAcceleration(bx2);
840 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
841 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
842 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
843 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
844 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
845 final Acceleration by2 = new Acceleration(0.0,
846 AccelerationUnit.FEET_PER_SQUARED_SECOND);
847 calibrator.getBiasYAsAcceleration(by2);
848 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
849 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
850 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
851 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
852 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
853 final Acceleration bz2 = new Acceleration(0.0,
854 AccelerationUnit.FEET_PER_SQUARED_SECOND);
855 calibrator.getBiasZAsAcceleration(bz2);
856 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
857 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
858 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
859 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
860 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
861 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
862 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
863 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
864 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
865 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
866 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
867 final double[] bias1 = calibrator.getBias();
868 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
869 final double[] bias2 = new double[3];
870 calibrator.getBias(bias2);
871 assertArrayEquals(bias1, bias2, 0.0);
872 final Matrix b1 = calibrator.getBiasAsMatrix();
873 assertEquals(b1, ba);
874 final Matrix b2 = new Matrix(3, 1);
875 calibrator.getBiasAsMatrix(b2);
876 assertEquals(b1, b2);
877 final Matrix ma1 = calibrator.getInitialMa();
878 assertEquals(ma1, new Matrix(3, 3));
879 final Matrix ma2 = new Matrix(3, 3);
880 calibrator.getInitialMa(ma2);
881 assertEquals(ma1, ma2);
882 assertNull(calibrator.getMeasurements());
883 assertFalse(calibrator.isCommonAxisUsed());
884 assertSame(calibrator.getListener(), this);
885 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
886 assertFalse(calibrator.isReady());
887 assertFalse(calibrator.isRunning());
888 assertNull(calibrator.getEstimatedMa());
889 assertNull(calibrator.getEstimatedSx());
890 assertNull(calibrator.getEstimatedSy());
891 assertNull(calibrator.getEstimatedSz());
892 assertNull(calibrator.getEstimatedMxy());
893 assertNull(calibrator.getEstimatedMxz());
894 assertNull(calibrator.getEstimatedMyx());
895 assertNull(calibrator.getEstimatedMyz());
896 assertNull(calibrator.getEstimatedMzx());
897 assertNull(calibrator.getEstimatedMzy());
898 assertNull(calibrator.getEstimatedCovariance());
899 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
900 assertNull(calibrator.getGroundTruthGravityNorm());
901 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
902 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
903 }
904
905 @Test
906 public void testConstructor11() throws WrongSizeException {
907 final Collection<StandardDeviationBodyKinematics> measurements =
908 Collections.emptyList();
909
910 final Matrix ba = generateBa();
911 final double biasX = ba.getElementAtIndex(0);
912 final double biasY = ba.getElementAtIndex(1);
913 final double biasZ = ba.getElementAtIndex(2);
914
915 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
916 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
917 biasX, biasY, biasZ);
918
919
920 assertEquals(calibrator.getBiasX(), biasX, 0.0);
921 assertEquals(calibrator.getBiasY(), biasY, 0.0);
922 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
923 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
924 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
926 final Acceleration bx2 = new Acceleration(0.0,
927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
928 calibrator.getBiasXAsAcceleration(bx2);
929 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
930 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
931 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
932 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
933 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
934 final Acceleration by2 = new Acceleration(0.0,
935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
936 calibrator.getBiasYAsAcceleration(by2);
937 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
938 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
939 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
940 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
941 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
942 final Acceleration bz2 = new Acceleration(0.0,
943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
944 calibrator.getBiasZAsAcceleration(bz2);
945 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
946 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
947 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
948 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
949 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
950 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
951 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
952 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
953 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
954 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
955 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
956 final double[] bias1 = calibrator.getBias();
957 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
958 final double[] bias2 = new double[3];
959 calibrator.getBias(bias2);
960 assertArrayEquals(bias1, bias2, 0.0);
961 final Matrix b1 = calibrator.getBiasAsMatrix();
962 assertEquals(b1, ba);
963 final Matrix b2 = new Matrix(3, 1);
964 calibrator.getBiasAsMatrix(b2);
965 assertEquals(b1, b2);
966 final Matrix ma1 = calibrator.getInitialMa();
967 assertEquals(ma1, new Matrix(3, 3));
968 final Matrix ma2 = new Matrix(3, 3);
969 calibrator.getInitialMa(ma2);
970 assertEquals(ma1, ma2);
971 assertSame(calibrator.getMeasurements(), measurements);
972 assertFalse(calibrator.isCommonAxisUsed());
973 assertNull(calibrator.getListener());
974 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
975 assertFalse(calibrator.isReady());
976 assertFalse(calibrator.isRunning());
977 assertNull(calibrator.getEstimatedMa());
978 assertNull(calibrator.getEstimatedSx());
979 assertNull(calibrator.getEstimatedSy());
980 assertNull(calibrator.getEstimatedSz());
981 assertNull(calibrator.getEstimatedMxy());
982 assertNull(calibrator.getEstimatedMxz());
983 assertNull(calibrator.getEstimatedMyx());
984 assertNull(calibrator.getEstimatedMyz());
985 assertNull(calibrator.getEstimatedMzx());
986 assertNull(calibrator.getEstimatedMzy());
987 assertNull(calibrator.getEstimatedCovariance());
988 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
989 assertNull(calibrator.getGroundTruthGravityNorm());
990 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
991 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
992 }
993
994 @Test
995 public void testConstructor12() throws WrongSizeException {
996 final Collection<StandardDeviationBodyKinematics> measurements =
997 Collections.emptyList();
998
999 final Matrix ba = generateBa();
1000 final double biasX = ba.getElementAtIndex(0);
1001 final double biasY = ba.getElementAtIndex(1);
1002 final double biasZ = ba.getElementAtIndex(2);
1003
1004 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1005 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1006 biasX, biasY, biasZ, this);
1007
1008
1009 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1010 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1011 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1012 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1013 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1014 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1015 final Acceleration bx2 = new Acceleration(0.0,
1016 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1017 calibrator.getBiasXAsAcceleration(bx2);
1018 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1019 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1020 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1021 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1022 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1023 final Acceleration by2 = new Acceleration(0.0,
1024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1025 calibrator.getBiasYAsAcceleration(by2);
1026 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1027 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1028 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1029 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1030 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1031 final Acceleration bz2 = new Acceleration(0.0,
1032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1033 calibrator.getBiasZAsAcceleration(bz2);
1034 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1035 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1036 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1037 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1038 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1039 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1040 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1041 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1042 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1043 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1044 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1045 final double[] bias1 = calibrator.getBias();
1046 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1047 final double[] bias2 = new double[3];
1048 calibrator.getBias(bias2);
1049 assertArrayEquals(bias1, bias2, 0.0);
1050 final Matrix b1 = calibrator.getBiasAsMatrix();
1051 assertEquals(b1, ba);
1052 final Matrix b2 = new Matrix(3, 1);
1053 calibrator.getBiasAsMatrix(b2);
1054 assertEquals(b1, b2);
1055 final Matrix ma1 = calibrator.getInitialMa();
1056 assertEquals(ma1, new Matrix(3, 3));
1057 final Matrix ma2 = new Matrix(3, 3);
1058 calibrator.getInitialMa(ma2);
1059 assertEquals(ma1, ma2);
1060 assertSame(calibrator.getMeasurements(), measurements);
1061 assertFalse(calibrator.isCommonAxisUsed());
1062 assertSame(calibrator.getListener(), this);
1063 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1064 assertFalse(calibrator.isReady());
1065 assertFalse(calibrator.isRunning());
1066 assertNull(calibrator.getEstimatedMa());
1067 assertNull(calibrator.getEstimatedSx());
1068 assertNull(calibrator.getEstimatedSy());
1069 assertNull(calibrator.getEstimatedSz());
1070 assertNull(calibrator.getEstimatedMxy());
1071 assertNull(calibrator.getEstimatedMxz());
1072 assertNull(calibrator.getEstimatedMyx());
1073 assertNull(calibrator.getEstimatedMyz());
1074 assertNull(calibrator.getEstimatedMzx());
1075 assertNull(calibrator.getEstimatedMzy());
1076 assertNull(calibrator.getEstimatedCovariance());
1077 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1078 assertNull(calibrator.getGroundTruthGravityNorm());
1079 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1080 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1081 }
1082
1083 @Test
1084 public void testConstructor13() throws WrongSizeException {
1085 final Matrix ba = generateBa();
1086 final double biasX = ba.getElementAtIndex(0);
1087 final double biasY = ba.getElementAtIndex(1);
1088 final double biasZ = ba.getElementAtIndex(2);
1089
1090 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1091 new KnownBiasAndGravityNormAccelerometerCalibrator(
1092 true, biasX, biasY, biasZ);
1093
1094
1095 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1096 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1097 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1098 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1099 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1100 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1101 final Acceleration bx2 = new Acceleration(0.0,
1102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1103 calibrator.getBiasXAsAcceleration(bx2);
1104 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1105 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1106 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1107 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1108 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1109 final Acceleration by2 = new Acceleration(0.0,
1110 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1111 calibrator.getBiasYAsAcceleration(by2);
1112 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1113 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1114 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1115 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1116 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1117 final Acceleration bz2 = new Acceleration(0.0,
1118 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1119 calibrator.getBiasZAsAcceleration(bz2);
1120 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1121 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1122 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1123 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1124 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1125 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1126 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1127 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1128 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1129 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1130 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1131 final double[] bias1 = calibrator.getBias();
1132 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1133 final double[] bias2 = new double[3];
1134 calibrator.getBias(bias2);
1135 assertArrayEquals(bias1, bias2, 0.0);
1136 final Matrix b1 = calibrator.getBiasAsMatrix();
1137 assertEquals(b1, ba);
1138 final Matrix b2 = new Matrix(3, 1);
1139 calibrator.getBiasAsMatrix(b2);
1140 assertEquals(b1, b2);
1141 final Matrix ma1 = calibrator.getInitialMa();
1142 assertEquals(ma1, new Matrix(3, 3));
1143 final Matrix ma2 = new Matrix(3, 3);
1144 calibrator.getInitialMa(ma2);
1145 assertEquals(ma1, ma2);
1146 assertNull(calibrator.getMeasurements());
1147 assertTrue(calibrator.isCommonAxisUsed());
1148 assertNull(calibrator.getListener());
1149 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1150 assertFalse(calibrator.isReady());
1151 assertFalse(calibrator.isRunning());
1152 assertNull(calibrator.getEstimatedMa());
1153 assertNull(calibrator.getEstimatedSx());
1154 assertNull(calibrator.getEstimatedSy());
1155 assertNull(calibrator.getEstimatedSz());
1156 assertNull(calibrator.getEstimatedMxy());
1157 assertNull(calibrator.getEstimatedMxz());
1158 assertNull(calibrator.getEstimatedMyx());
1159 assertNull(calibrator.getEstimatedMyz());
1160 assertNull(calibrator.getEstimatedMzx());
1161 assertNull(calibrator.getEstimatedMzy());
1162 assertNull(calibrator.getEstimatedCovariance());
1163 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1164 assertNull(calibrator.getGroundTruthGravityNorm());
1165 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1166 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1167 }
1168
1169 @Test
1170 public void testConstructor14() throws WrongSizeException {
1171 final Matrix ba = generateBa();
1172 final double biasX = ba.getElementAtIndex(0);
1173 final double biasY = ba.getElementAtIndex(1);
1174 final double biasZ = ba.getElementAtIndex(2);
1175
1176 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1177 new KnownBiasAndGravityNormAccelerometerCalibrator(
1178 true, biasX, biasY, biasZ,
1179 this);
1180
1181
1182 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1183 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1184 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1185 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1186 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1187 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1188 final Acceleration bx2 = new Acceleration(0.0,
1189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1190 calibrator.getBiasXAsAcceleration(bx2);
1191 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1192 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1193 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1194 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1195 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1196 final Acceleration by2 = new Acceleration(0.0,
1197 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1198 calibrator.getBiasYAsAcceleration(by2);
1199 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1200 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1201 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1202 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1203 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1204 final Acceleration bz2 = new Acceleration(0.0,
1205 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1206 calibrator.getBiasZAsAcceleration(bz2);
1207 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1208 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1209 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1210 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1211 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1212 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1213 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1214 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1215 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1216 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1217 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1218 final double[] bias1 = calibrator.getBias();
1219 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1220 final double[] bias2 = new double[3];
1221 calibrator.getBias(bias2);
1222 assertArrayEquals(bias1, bias2, 0.0);
1223 final Matrix b1 = calibrator.getBiasAsMatrix();
1224 assertEquals(b1, ba);
1225 final Matrix b2 = new Matrix(3, 1);
1226 calibrator.getBiasAsMatrix(b2);
1227 assertEquals(b1, b2);
1228 final Matrix ma1 = calibrator.getInitialMa();
1229 assertEquals(ma1, new Matrix(3, 3));
1230 final Matrix ma2 = new Matrix(3, 3);
1231 calibrator.getInitialMa(ma2);
1232 assertEquals(ma1, ma2);
1233 assertNull(calibrator.getMeasurements());
1234 assertTrue(calibrator.isCommonAxisUsed());
1235 assertSame(calibrator.getListener(), this);
1236 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1237 assertFalse(calibrator.isReady());
1238 assertFalse(calibrator.isRunning());
1239 assertNull(calibrator.getEstimatedMa());
1240 assertNull(calibrator.getEstimatedSx());
1241 assertNull(calibrator.getEstimatedSy());
1242 assertNull(calibrator.getEstimatedSz());
1243 assertNull(calibrator.getEstimatedMxy());
1244 assertNull(calibrator.getEstimatedMxz());
1245 assertNull(calibrator.getEstimatedMyx());
1246 assertNull(calibrator.getEstimatedMyz());
1247 assertNull(calibrator.getEstimatedMzx());
1248 assertNull(calibrator.getEstimatedMzy());
1249 assertNull(calibrator.getEstimatedCovariance());
1250 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1251 assertNull(calibrator.getGroundTruthGravityNorm());
1252 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1253 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1254 }
1255
1256 @Test
1257 public void testConstructor15() throws WrongSizeException {
1258 final Collection<StandardDeviationBodyKinematics> measurements =
1259 Collections.emptyList();
1260
1261 final Matrix ba = generateBa();
1262 final double biasX = ba.getElementAtIndex(0);
1263 final double biasY = ba.getElementAtIndex(1);
1264 final double biasZ = ba.getElementAtIndex(2);
1265
1266 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1267 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1268 true, biasX, biasY, biasZ);
1269
1270
1271 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1272 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1273 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1274 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1275 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1276 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1277 final Acceleration bx2 = new Acceleration(0.0,
1278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1279 calibrator.getBiasXAsAcceleration(bx2);
1280 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1281 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1282 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1283 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1284 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1285 final Acceleration by2 = new Acceleration(0.0,
1286 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1287 calibrator.getBiasYAsAcceleration(by2);
1288 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1289 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1290 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1291 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1292 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1293 final Acceleration bz2 = new Acceleration(0.0,
1294 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1295 calibrator.getBiasZAsAcceleration(bz2);
1296 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1297 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1298 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1299 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1300 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1301 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1302 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1303 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1304 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1305 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1306 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1307 final double[] bias1 = calibrator.getBias();
1308 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1309 final double[] bias2 = new double[3];
1310 calibrator.getBias(bias2);
1311 assertArrayEquals(bias1, bias2, 0.0);
1312 final Matrix b1 = calibrator.getBiasAsMatrix();
1313 assertEquals(b1, ba);
1314 final Matrix b2 = new Matrix(3, 1);
1315 calibrator.getBiasAsMatrix(b2);
1316 assertEquals(b1, b2);
1317 final Matrix ma1 = calibrator.getInitialMa();
1318 assertEquals(ma1, new Matrix(3, 3));
1319 final Matrix ma2 = new Matrix(3, 3);
1320 calibrator.getInitialMa(ma2);
1321 assertEquals(ma1, ma2);
1322 assertSame(calibrator.getMeasurements(), measurements);
1323 assertTrue(calibrator.isCommonAxisUsed());
1324 assertNull(calibrator.getListener());
1325 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1326 assertFalse(calibrator.isReady());
1327 assertFalse(calibrator.isRunning());
1328 assertNull(calibrator.getEstimatedMa());
1329 assertNull(calibrator.getEstimatedSx());
1330 assertNull(calibrator.getEstimatedSy());
1331 assertNull(calibrator.getEstimatedSz());
1332 assertNull(calibrator.getEstimatedMxy());
1333 assertNull(calibrator.getEstimatedMxz());
1334 assertNull(calibrator.getEstimatedMyx());
1335 assertNull(calibrator.getEstimatedMyz());
1336 assertNull(calibrator.getEstimatedMzx());
1337 assertNull(calibrator.getEstimatedMzy());
1338 assertNull(calibrator.getEstimatedCovariance());
1339 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1340 assertNull(calibrator.getGroundTruthGravityNorm());
1341 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1342 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1343 }
1344
1345 @Test
1346 public void testConstructor16() throws WrongSizeException {
1347 final Collection<StandardDeviationBodyKinematics> measurements =
1348 Collections.emptyList();
1349
1350 final Matrix ba = generateBa();
1351 final double biasX = ba.getElementAtIndex(0);
1352 final double biasY = ba.getElementAtIndex(1);
1353 final double biasZ = ba.getElementAtIndex(2);
1354
1355 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1356 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1357 true, biasX, biasY, biasZ, this);
1358
1359
1360 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1361 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1362 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1363 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1364 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1365 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1366 final Acceleration bx2 = new Acceleration(0.0,
1367 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1368 calibrator.getBiasXAsAcceleration(bx2);
1369 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1370 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1371 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1372 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1373 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1374 final Acceleration by2 = new Acceleration(0.0,
1375 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1376 calibrator.getBiasYAsAcceleration(by2);
1377 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1378 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1379 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1380 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1381 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1382 final Acceleration bz2 = new Acceleration(0.0,
1383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1384 calibrator.getBiasZAsAcceleration(bz2);
1385 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1386 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1387 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1388 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1389 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1390 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1391 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1392 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1393 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1394 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1395 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1396 final double[] bias1 = calibrator.getBias();
1397 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1398 final double[] bias2 = new double[3];
1399 calibrator.getBias(bias2);
1400 assertArrayEquals(bias1, bias2, 0.0);
1401 final Matrix b1 = calibrator.getBiasAsMatrix();
1402 assertEquals(b1, ba);
1403 final Matrix b2 = new Matrix(3, 1);
1404 calibrator.getBiasAsMatrix(b2);
1405 assertEquals(b1, b2);
1406 final Matrix ma1 = calibrator.getInitialMa();
1407 assertEquals(ma1, new Matrix(3, 3));
1408 final Matrix ma2 = new Matrix(3, 3);
1409 calibrator.getInitialMa(ma2);
1410 assertEquals(ma1, ma2);
1411 assertSame(calibrator.getMeasurements(), measurements);
1412 assertTrue(calibrator.isCommonAxisUsed());
1413 assertSame(calibrator.getListener(), this);
1414 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1415 assertFalse(calibrator.isReady());
1416 assertFalse(calibrator.isRunning());
1417 assertNull(calibrator.getEstimatedMa());
1418 assertNull(calibrator.getEstimatedSx());
1419 assertNull(calibrator.getEstimatedSy());
1420 assertNull(calibrator.getEstimatedSz());
1421 assertNull(calibrator.getEstimatedMxy());
1422 assertNull(calibrator.getEstimatedMxz());
1423 assertNull(calibrator.getEstimatedMyx());
1424 assertNull(calibrator.getEstimatedMyz());
1425 assertNull(calibrator.getEstimatedMzx());
1426 assertNull(calibrator.getEstimatedMzy());
1427 assertNull(calibrator.getEstimatedCovariance());
1428 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1429 assertNull(calibrator.getGroundTruthGravityNorm());
1430 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1431 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1432 }
1433
1434 @Test
1435 public void testConstructor17() throws WrongSizeException {
1436 final Matrix ba = generateBa();
1437 final double biasX = ba.getElementAtIndex(0);
1438 final double biasY = ba.getElementAtIndex(1);
1439 final double biasZ = ba.getElementAtIndex(2);
1440
1441 final Acceleration bx = new Acceleration(biasX,
1442 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1443 final Acceleration by = new Acceleration(biasY,
1444 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445 final Acceleration bz = new Acceleration(biasZ,
1446 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1447
1448 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1449 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz);
1450
1451
1452 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1453 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1454 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1455 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1456 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1457 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1458 final Acceleration bx2 = new Acceleration(0.0,
1459 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1460 calibrator.getBiasXAsAcceleration(bx2);
1461 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1462 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1463 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1464 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1465 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1466 final Acceleration by2 = new Acceleration(0.0,
1467 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1468 calibrator.getBiasYAsAcceleration(by2);
1469 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1470 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1471 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1472 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1473 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1474 final Acceleration bz2 = new Acceleration(0.0,
1475 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1476 calibrator.getBiasZAsAcceleration(bz2);
1477 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1478 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1479 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1480 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1481 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1482 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1483 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1484 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1485 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1486 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1487 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1488 final double[] bias1 = calibrator.getBias();
1489 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1490 final double[] bias2 = new double[3];
1491 calibrator.getBias(bias2);
1492 assertArrayEquals(bias1, bias2, 0.0);
1493 final Matrix b1 = calibrator.getBiasAsMatrix();
1494 assertEquals(b1, ba);
1495 final Matrix b2 = new Matrix(3, 1);
1496 calibrator.getBiasAsMatrix(b2);
1497 assertEquals(b1, b2);
1498 final Matrix ma1 = calibrator.getInitialMa();
1499 assertEquals(ma1, new Matrix(3, 3));
1500 final Matrix ma2 = new Matrix(3, 3);
1501 calibrator.getInitialMa(ma2);
1502 assertEquals(ma1, ma2);
1503 assertNull(calibrator.getMeasurements());
1504 assertFalse(calibrator.isCommonAxisUsed());
1505 assertNull(calibrator.getListener());
1506 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1507 assertFalse(calibrator.isReady());
1508 assertFalse(calibrator.isRunning());
1509 assertNull(calibrator.getEstimatedMa());
1510 assertNull(calibrator.getEstimatedSx());
1511 assertNull(calibrator.getEstimatedSy());
1512 assertNull(calibrator.getEstimatedSz());
1513 assertNull(calibrator.getEstimatedMxy());
1514 assertNull(calibrator.getEstimatedMxz());
1515 assertNull(calibrator.getEstimatedMyx());
1516 assertNull(calibrator.getEstimatedMyz());
1517 assertNull(calibrator.getEstimatedMzx());
1518 assertNull(calibrator.getEstimatedMzy());
1519 assertNull(calibrator.getEstimatedCovariance());
1520 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1521 assertNull(calibrator.getGroundTruthGravityNorm());
1522 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1523 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1524 }
1525
1526 @Test
1527 public void testConstructor18() throws WrongSizeException {
1528 final Matrix ba = generateBa();
1529 final double biasX = ba.getElementAtIndex(0);
1530 final double biasY = ba.getElementAtIndex(1);
1531 final double biasZ = ba.getElementAtIndex(2);
1532
1533 final Acceleration bx = new Acceleration(biasX,
1534 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1535 final Acceleration by = new Acceleration(biasY,
1536 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1537 final Acceleration bz = new Acceleration(biasZ,
1538 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1539
1540 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1541 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
1542 this);
1543
1544
1545 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1546 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1547 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1548 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1549 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1550 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1551 final Acceleration bx2 = new Acceleration(0.0,
1552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1553 calibrator.getBiasXAsAcceleration(bx2);
1554 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1555 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1556 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1557 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1558 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1559 final Acceleration by2 = new Acceleration(0.0,
1560 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1561 calibrator.getBiasYAsAcceleration(by2);
1562 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1563 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1564 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1565 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1566 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1567 final Acceleration bz2 = new Acceleration(0.0,
1568 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1569 calibrator.getBiasZAsAcceleration(bz2);
1570 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1571 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1572 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1573 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1574 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1575 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1576 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1577 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1578 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1579 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1580 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1581 final double[] bias1 = calibrator.getBias();
1582 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1583 final double[] bias2 = new double[3];
1584 calibrator.getBias(bias2);
1585 assertArrayEquals(bias1, bias2, 0.0);
1586 final Matrix b1 = calibrator.getBiasAsMatrix();
1587 assertEquals(b1, ba);
1588 final Matrix b2 = new Matrix(3, 1);
1589 calibrator.getBiasAsMatrix(b2);
1590 assertEquals(b1, b2);
1591 final Matrix ma1 = calibrator.getInitialMa();
1592 assertEquals(ma1, new Matrix(3, 3));
1593 final Matrix ma2 = new Matrix(3, 3);
1594 calibrator.getInitialMa(ma2);
1595 assertEquals(ma1, ma2);
1596 assertNull(calibrator.getMeasurements());
1597 assertFalse(calibrator.isCommonAxisUsed());
1598 assertSame(calibrator.getListener(), this);
1599 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1600 assertFalse(calibrator.isReady());
1601 assertFalse(calibrator.isRunning());
1602 assertNull(calibrator.getEstimatedMa());
1603 assertNull(calibrator.getEstimatedSx());
1604 assertNull(calibrator.getEstimatedSy());
1605 assertNull(calibrator.getEstimatedSz());
1606 assertNull(calibrator.getEstimatedMxy());
1607 assertNull(calibrator.getEstimatedMxz());
1608 assertNull(calibrator.getEstimatedMyx());
1609 assertNull(calibrator.getEstimatedMyz());
1610 assertNull(calibrator.getEstimatedMzx());
1611 assertNull(calibrator.getEstimatedMzy());
1612 assertNull(calibrator.getEstimatedCovariance());
1613 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1614 assertNull(calibrator.getGroundTruthGravityNorm());
1615 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1616 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1617 }
1618
1619 @Test
1620 public void testConstructor19() throws WrongSizeException {
1621 final Collection<StandardDeviationBodyKinematics> measurements =
1622 Collections.emptyList();
1623
1624 final Matrix ba = generateBa();
1625 final double biasX = ba.getElementAtIndex(0);
1626 final double biasY = ba.getElementAtIndex(1);
1627 final double biasZ = ba.getElementAtIndex(2);
1628
1629 final Acceleration bx = new Acceleration(biasX,
1630 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1631 final Acceleration by = new Acceleration(biasY,
1632 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1633 final Acceleration bz = new Acceleration(biasZ,
1634 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1635
1636 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1637 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1638 bx, by, bz);
1639
1640
1641 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1642 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1643 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1644 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1645 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1646 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1647 final Acceleration bx2 = new Acceleration(0.0,
1648 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1649 calibrator.getBiasXAsAcceleration(bx2);
1650 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1651 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1653 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1654 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1655 final Acceleration by2 = new Acceleration(0.0,
1656 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1657 calibrator.getBiasYAsAcceleration(by2);
1658 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1659 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1661 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1662 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1663 final Acceleration bz2 = new Acceleration(0.0,
1664 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1665 calibrator.getBiasZAsAcceleration(bz2);
1666 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1667 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1669 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1670 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1671 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1672 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1673 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1674 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1675 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1676 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1677 final double[] bias1 = calibrator.getBias();
1678 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1679 final double[] bias2 = new double[3];
1680 calibrator.getBias(bias2);
1681 assertArrayEquals(bias1, bias2, 0.0);
1682 final Matrix b1 = calibrator.getBiasAsMatrix();
1683 assertEquals(b1, ba);
1684 final Matrix b2 = new Matrix(3, 1);
1685 calibrator.getBiasAsMatrix(b2);
1686 assertEquals(b1, b2);
1687 final Matrix ma1 = calibrator.getInitialMa();
1688 assertEquals(ma1, new Matrix(3, 3));
1689 final Matrix ma2 = new Matrix(3, 3);
1690 calibrator.getInitialMa(ma2);
1691 assertEquals(ma1, ma2);
1692 assertSame(calibrator.getMeasurements(), measurements);
1693 assertFalse(calibrator.isCommonAxisUsed());
1694 assertNull(calibrator.getListener());
1695 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1696 assertFalse(calibrator.isReady());
1697 assertFalse(calibrator.isRunning());
1698 assertNull(calibrator.getEstimatedMa());
1699 assertNull(calibrator.getEstimatedSx());
1700 assertNull(calibrator.getEstimatedSy());
1701 assertNull(calibrator.getEstimatedSz());
1702 assertNull(calibrator.getEstimatedMxy());
1703 assertNull(calibrator.getEstimatedMxz());
1704 assertNull(calibrator.getEstimatedMyx());
1705 assertNull(calibrator.getEstimatedMyz());
1706 assertNull(calibrator.getEstimatedMzx());
1707 assertNull(calibrator.getEstimatedMzy());
1708 assertNull(calibrator.getEstimatedCovariance());
1709 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1710 assertNull(calibrator.getGroundTruthGravityNorm());
1711 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1712 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1713 }
1714
1715 @Test
1716 public void testConstructor20() throws WrongSizeException {
1717 final Collection<StandardDeviationBodyKinematics> measurements =
1718 Collections.emptyList();
1719
1720 final Matrix ba = generateBa();
1721 final double biasX = ba.getElementAtIndex(0);
1722 final double biasY = ba.getElementAtIndex(1);
1723 final double biasZ = ba.getElementAtIndex(2);
1724
1725 final Acceleration bx = new Acceleration(biasX,
1726 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1727 final Acceleration by = new Acceleration(biasY,
1728 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1729 final Acceleration bz = new Acceleration(biasZ,
1730 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1731
1732 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1733 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1734 bx, by, bz, this);
1735
1736
1737 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1738 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1739 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1740 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1741 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1742 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1743 final Acceleration bx2 = new Acceleration(0.0,
1744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1745 calibrator.getBiasXAsAcceleration(bx2);
1746 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1747 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1748 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1749 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1750 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1751 final Acceleration by2 = new Acceleration(0.0,
1752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1753 calibrator.getBiasYAsAcceleration(by2);
1754 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1755 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1756 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1757 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1758 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1759 final Acceleration bz2 = new Acceleration(0.0,
1760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1761 calibrator.getBiasZAsAcceleration(bz2);
1762 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1763 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1764 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1765 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1766 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1767 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1768 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1769 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1770 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1771 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1772 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1773 final double[] bias1 = calibrator.getBias();
1774 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1775 final double[] bias2 = new double[3];
1776 calibrator.getBias(bias2);
1777 assertArrayEquals(bias1, bias2, 0.0);
1778 final Matrix b1 = calibrator.getBiasAsMatrix();
1779 assertEquals(b1, ba);
1780 final Matrix b2 = new Matrix(3, 1);
1781 calibrator.getBiasAsMatrix(b2);
1782 assertEquals(b1, b2);
1783 final Matrix ma1 = calibrator.getInitialMa();
1784 assertEquals(ma1, new Matrix(3, 3));
1785 final Matrix ma2 = new Matrix(3, 3);
1786 calibrator.getInitialMa(ma2);
1787 assertEquals(ma1, ma2);
1788 assertSame(calibrator.getMeasurements(), measurements);
1789 assertFalse(calibrator.isCommonAxisUsed());
1790 assertSame(calibrator.getListener(), this);
1791 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1792 assertFalse(calibrator.isReady());
1793 assertFalse(calibrator.isRunning());
1794 assertNull(calibrator.getEstimatedMa());
1795 assertNull(calibrator.getEstimatedSx());
1796 assertNull(calibrator.getEstimatedSy());
1797 assertNull(calibrator.getEstimatedSz());
1798 assertNull(calibrator.getEstimatedMxy());
1799 assertNull(calibrator.getEstimatedMxz());
1800 assertNull(calibrator.getEstimatedMyx());
1801 assertNull(calibrator.getEstimatedMyz());
1802 assertNull(calibrator.getEstimatedMzx());
1803 assertNull(calibrator.getEstimatedMzy());
1804 assertNull(calibrator.getEstimatedCovariance());
1805 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1806 assertNull(calibrator.getGroundTruthGravityNorm());
1807 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1808 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1809 }
1810
1811 @Test
1812 public void testConstructor21() throws WrongSizeException {
1813 final Matrix ba = generateBa();
1814 final double biasX = ba.getElementAtIndex(0);
1815 final double biasY = ba.getElementAtIndex(1);
1816 final double biasZ = ba.getElementAtIndex(2);
1817
1818 final Acceleration bx = new Acceleration(biasX,
1819 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1820 final Acceleration by = new Acceleration(biasY,
1821 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1822 final Acceleration bz = new Acceleration(biasZ,
1823 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1824
1825 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1826 new KnownBiasAndGravityNormAccelerometerCalibrator(
1827 true, bx, by, bz);
1828
1829
1830 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1831 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1832 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1833 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1834 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1835 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1836 final Acceleration bx2 = new Acceleration(0.0,
1837 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1838 calibrator.getBiasXAsAcceleration(bx2);
1839 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1840 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1841 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1842 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1843 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1844 final Acceleration by2 = new Acceleration(0.0,
1845 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1846 calibrator.getBiasYAsAcceleration(by2);
1847 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1848 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1849 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1850 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1851 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1852 final Acceleration bz2 = new Acceleration(0.0,
1853 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1854 calibrator.getBiasZAsAcceleration(bz2);
1855 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1856 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1857 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1858 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1859 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1860 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1861 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1862 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1863 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1864 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1865 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1866 final double[] bias1 = calibrator.getBias();
1867 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1868 final double[] bias2 = new double[3];
1869 calibrator.getBias(bias2);
1870 assertArrayEquals(bias1, bias2, 0.0);
1871 final Matrix b1 = calibrator.getBiasAsMatrix();
1872 assertEquals(b1, ba);
1873 final Matrix b2 = new Matrix(3, 1);
1874 calibrator.getBiasAsMatrix(b2);
1875 assertEquals(b1, b2);
1876 final Matrix ma1 = calibrator.getInitialMa();
1877 assertEquals(ma1, new Matrix(3, 3));
1878 final Matrix ma2 = new Matrix(3, 3);
1879 calibrator.getInitialMa(ma2);
1880 assertEquals(ma1, ma2);
1881 assertNull(calibrator.getMeasurements());
1882 assertTrue(calibrator.isCommonAxisUsed());
1883 assertNull(calibrator.getListener());
1884 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1885 assertFalse(calibrator.isReady());
1886 assertFalse(calibrator.isRunning());
1887 assertNull(calibrator.getEstimatedMa());
1888 assertNull(calibrator.getEstimatedSx());
1889 assertNull(calibrator.getEstimatedSy());
1890 assertNull(calibrator.getEstimatedSz());
1891 assertNull(calibrator.getEstimatedMxy());
1892 assertNull(calibrator.getEstimatedMxz());
1893 assertNull(calibrator.getEstimatedMyx());
1894 assertNull(calibrator.getEstimatedMyz());
1895 assertNull(calibrator.getEstimatedMzx());
1896 assertNull(calibrator.getEstimatedMzy());
1897 assertNull(calibrator.getEstimatedCovariance());
1898 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1899 assertNull(calibrator.getGroundTruthGravityNorm());
1900 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1901 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1902 }
1903
1904 @Test
1905 public void testConstructor22() throws WrongSizeException {
1906 final Matrix ba = generateBa();
1907 final double biasX = ba.getElementAtIndex(0);
1908 final double biasY = ba.getElementAtIndex(1);
1909 final double biasZ = ba.getElementAtIndex(2);
1910
1911 final Acceleration bx = new Acceleration(biasX,
1912 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1913 final Acceleration by = new Acceleration(biasY,
1914 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1915 final Acceleration bz = new Acceleration(biasZ,
1916 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1917
1918 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1919 new KnownBiasAndGravityNormAccelerometerCalibrator(
1920 true, bx, by, bz, this);
1921
1922
1923 assertEquals(calibrator.getBiasX(), biasX, 0.0);
1924 assertEquals(calibrator.getBiasY(), biasY, 0.0);
1925 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1926 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1927 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1928 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1929 final Acceleration bx2 = new Acceleration(0.0,
1930 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1931 calibrator.getBiasXAsAcceleration(bx2);
1932 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1933 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1934 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1935 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1936 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1937 final Acceleration by2 = new Acceleration(0.0,
1938 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1939 calibrator.getBiasYAsAcceleration(by2);
1940 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1941 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1942 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1943 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1944 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1945 final Acceleration bz2 = new Acceleration(0.0,
1946 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1947 calibrator.getBiasZAsAcceleration(bz2);
1948 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1949 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1950 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1951 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1952 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1953 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1954 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1955 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1956 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1957 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1958 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1959 final double[] bias1 = calibrator.getBias();
1960 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1961 final double[] bias2 = new double[3];
1962 calibrator.getBias(bias2);
1963 assertArrayEquals(bias1, bias2, 0.0);
1964 final Matrix b1 = calibrator.getBiasAsMatrix();
1965 assertEquals(b1, ba);
1966 final Matrix b2 = new Matrix(3, 1);
1967 calibrator.getBiasAsMatrix(b2);
1968 assertEquals(b1, b2);
1969 final Matrix ma1 = calibrator.getInitialMa();
1970 assertEquals(ma1, new Matrix(3, 3));
1971 final Matrix ma2 = new Matrix(3, 3);
1972 calibrator.getInitialMa(ma2);
1973 assertEquals(ma1, ma2);
1974 assertNull(calibrator.getMeasurements());
1975 assertTrue(calibrator.isCommonAxisUsed());
1976 assertSame(calibrator.getListener(), this);
1977 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1978 assertFalse(calibrator.isReady());
1979 assertFalse(calibrator.isRunning());
1980 assertNull(calibrator.getEstimatedMa());
1981 assertNull(calibrator.getEstimatedSx());
1982 assertNull(calibrator.getEstimatedSy());
1983 assertNull(calibrator.getEstimatedSz());
1984 assertNull(calibrator.getEstimatedMxy());
1985 assertNull(calibrator.getEstimatedMxz());
1986 assertNull(calibrator.getEstimatedMyx());
1987 assertNull(calibrator.getEstimatedMyz());
1988 assertNull(calibrator.getEstimatedMzx());
1989 assertNull(calibrator.getEstimatedMzy());
1990 assertNull(calibrator.getEstimatedCovariance());
1991 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1992 assertNull(calibrator.getGroundTruthGravityNorm());
1993 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1994 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1995 }
1996
1997 @Test
1998 public void testConstructor23() throws WrongSizeException {
1999 final Collection<StandardDeviationBodyKinematics> measurements =
2000 Collections.emptyList();
2001
2002 final Matrix ba = generateBa();
2003 final double biasX = ba.getElementAtIndex(0);
2004 final double biasY = ba.getElementAtIndex(1);
2005 final double biasZ = ba.getElementAtIndex(2);
2006
2007 final Acceleration bx = new Acceleration(biasX,
2008 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2009 final Acceleration by = new Acceleration(biasY,
2010 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2011 final Acceleration bz = new Acceleration(biasZ,
2012 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2013
2014 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2015 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2016 true, bx, by, bz);
2017
2018
2019 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2020 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2021 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2022 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2023 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2024 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2025 final Acceleration bx2 = new Acceleration(0.0,
2026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2027 calibrator.getBiasXAsAcceleration(bx2);
2028 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2029 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2030 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2031 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2032 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2033 final Acceleration by2 = new Acceleration(0.0,
2034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2035 calibrator.getBiasYAsAcceleration(by2);
2036 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2037 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2038 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2039 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2040 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2041 final Acceleration bz2 = new Acceleration(0.0,
2042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2043 calibrator.getBiasZAsAcceleration(bz2);
2044 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2045 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2046 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2047 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2048 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2049 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2050 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2051 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2052 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2053 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2054 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2055 final double[] bias1 = calibrator.getBias();
2056 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2057 final double[] bias2 = new double[3];
2058 calibrator.getBias(bias2);
2059 assertArrayEquals(bias1, bias2, 0.0);
2060 final Matrix b1 = calibrator.getBiasAsMatrix();
2061 assertEquals(b1, ba);
2062 final Matrix b2 = new Matrix(3, 1);
2063 calibrator.getBiasAsMatrix(b2);
2064 assertEquals(b1, b2);
2065 final Matrix ma1 = calibrator.getInitialMa();
2066 assertEquals(ma1, new Matrix(3, 3));
2067 final Matrix ma2 = new Matrix(3, 3);
2068 calibrator.getInitialMa(ma2);
2069 assertEquals(ma1, ma2);
2070 assertSame(calibrator.getMeasurements(), measurements);
2071 assertTrue(calibrator.isCommonAxisUsed());
2072 assertNull(calibrator.getListener());
2073 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2074 assertFalse(calibrator.isReady());
2075 assertFalse(calibrator.isRunning());
2076 assertNull(calibrator.getEstimatedMa());
2077 assertNull(calibrator.getEstimatedSx());
2078 assertNull(calibrator.getEstimatedSy());
2079 assertNull(calibrator.getEstimatedSz());
2080 assertNull(calibrator.getEstimatedMxy());
2081 assertNull(calibrator.getEstimatedMxz());
2082 assertNull(calibrator.getEstimatedMyx());
2083 assertNull(calibrator.getEstimatedMyz());
2084 assertNull(calibrator.getEstimatedMzx());
2085 assertNull(calibrator.getEstimatedMzy());
2086 assertNull(calibrator.getEstimatedCovariance());
2087 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2088 assertNull(calibrator.getGroundTruthGravityNorm());
2089 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2090 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2091 }
2092
2093 @Test
2094 public void testConstructor24() throws WrongSizeException {
2095 final Collection<StandardDeviationBodyKinematics> measurements =
2096 Collections.emptyList();
2097
2098 final Matrix ba = generateBa();
2099 final double biasX = ba.getElementAtIndex(0);
2100 final double biasY = ba.getElementAtIndex(1);
2101 final double biasZ = ba.getElementAtIndex(2);
2102
2103 final Acceleration bx = new Acceleration(biasX,
2104 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2105 final Acceleration by = new Acceleration(biasY,
2106 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2107 final Acceleration bz = new Acceleration(biasZ,
2108 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2109
2110 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2111 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2112 true, bx, by, bz, this);
2113
2114
2115 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2116 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2117 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2118 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2119 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2120 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2121 final Acceleration bx2 = new Acceleration(0.0,
2122 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2123 calibrator.getBiasXAsAcceleration(bx2);
2124 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2125 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2126 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2127 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2128 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2129 final Acceleration by2 = new Acceleration(0.0,
2130 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2131 calibrator.getBiasYAsAcceleration(by2);
2132 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2133 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2134 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2135 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2136 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2137 final Acceleration bz2 = new Acceleration(0.0,
2138 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2139 calibrator.getBiasZAsAcceleration(bz2);
2140 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2141 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2142 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2143 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2144 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2145 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2146 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2147 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2148 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2149 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2150 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2151 final double[] bias1 = calibrator.getBias();
2152 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2153 final double[] bias2 = new double[3];
2154 calibrator.getBias(bias2);
2155 assertArrayEquals(bias1, bias2, 0.0);
2156 final Matrix b1 = calibrator.getBiasAsMatrix();
2157 assertEquals(b1, ba);
2158 final Matrix b2 = new Matrix(3, 1);
2159 calibrator.getBiasAsMatrix(b2);
2160 assertEquals(b1, b2);
2161 final Matrix ma1 = calibrator.getInitialMa();
2162 assertEquals(ma1, new Matrix(3, 3));
2163 final Matrix ma2 = new Matrix(3, 3);
2164 calibrator.getInitialMa(ma2);
2165 assertEquals(ma1, ma2);
2166 assertSame(calibrator.getMeasurements(), measurements);
2167 assertTrue(calibrator.isCommonAxisUsed());
2168 assertSame(calibrator.getListener(), this);
2169 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2170 assertFalse(calibrator.isReady());
2171 assertFalse(calibrator.isRunning());
2172 assertNull(calibrator.getEstimatedMa());
2173 assertNull(calibrator.getEstimatedSx());
2174 assertNull(calibrator.getEstimatedSy());
2175 assertNull(calibrator.getEstimatedSz());
2176 assertNull(calibrator.getEstimatedMxy());
2177 assertNull(calibrator.getEstimatedMxz());
2178 assertNull(calibrator.getEstimatedMyx());
2179 assertNull(calibrator.getEstimatedMyz());
2180 assertNull(calibrator.getEstimatedMzx());
2181 assertNull(calibrator.getEstimatedMzy());
2182 assertNull(calibrator.getEstimatedCovariance());
2183 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2184 assertNull(calibrator.getGroundTruthGravityNorm());
2185 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2186 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2187 }
2188
2189 @Test
2190 public void testConstructor25() throws WrongSizeException {
2191 final Matrix ba = generateBa();
2192 final double biasX = ba.getElementAtIndex(0);
2193 final double biasY = ba.getElementAtIndex(1);
2194 final double biasZ = ba.getElementAtIndex(2);
2195
2196 final Matrix ma = generateMaCommonAxis();
2197 final double sx = ma.getElementAt(0, 0);
2198 final double sy = ma.getElementAt(1, 1);
2199 final double sz = ma.getElementAt(2, 2);
2200
2201 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2202 new KnownBiasAndGravityNormAccelerometerCalibrator(
2203 biasX, biasY, biasZ,
2204 sx, sy, sz);
2205
2206
2207 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2208 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2209 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2210 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2211 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2212 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2213 final Acceleration bx2 = new Acceleration(0.0,
2214 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2215 calibrator.getBiasXAsAcceleration(bx2);
2216 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2217 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2218 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2219 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2220 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2221 final Acceleration by2 = new Acceleration(0.0,
2222 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2223 calibrator.getBiasYAsAcceleration(by2);
2224 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2225 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2226 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2227 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2228 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2229 final Acceleration bz2 = new Acceleration(0.0,
2230 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2231 calibrator.getBiasZAsAcceleration(bz2);
2232 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2233 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2234 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2235 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2236 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2237 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2238 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2239 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2240 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2241 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2242 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2243 final double[] bias1 = calibrator.getBias();
2244 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2245 final double[] bias2 = new double[3];
2246 calibrator.getBias(bias2);
2247 assertArrayEquals(bias1, bias2, 0.0);
2248 final Matrix b1 = calibrator.getBiasAsMatrix();
2249 assertEquals(b1, ba);
2250 final Matrix b2 = new Matrix(3, 1);
2251 calibrator.getBiasAsMatrix(b2);
2252 assertEquals(b1, b2);
2253 final Matrix ma1 = calibrator.getInitialMa();
2254 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2255 final Matrix ma2 = new Matrix(3, 3);
2256 calibrator.getInitialMa(ma2);
2257 assertEquals(ma1, ma2);
2258 assertNull(calibrator.getMeasurements());
2259 assertFalse(calibrator.isCommonAxisUsed());
2260 assertNull(calibrator.getListener());
2261 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2262 assertFalse(calibrator.isReady());
2263 assertFalse(calibrator.isRunning());
2264 assertNull(calibrator.getEstimatedMa());
2265 assertNull(calibrator.getEstimatedSx());
2266 assertNull(calibrator.getEstimatedSy());
2267 assertNull(calibrator.getEstimatedSz());
2268 assertNull(calibrator.getEstimatedMxy());
2269 assertNull(calibrator.getEstimatedMxz());
2270 assertNull(calibrator.getEstimatedMyx());
2271 assertNull(calibrator.getEstimatedMyz());
2272 assertNull(calibrator.getEstimatedMzx());
2273 assertNull(calibrator.getEstimatedMzy());
2274 assertNull(calibrator.getEstimatedCovariance());
2275 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2276 assertNull(calibrator.getGroundTruthGravityNorm());
2277 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2278 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2279 }
2280
2281 @Test
2282 public void testConstructor26() throws WrongSizeException {
2283 final Collection<StandardDeviationBodyKinematics> measurements =
2284 Collections.emptyList();
2285
2286 final Matrix ba = generateBa();
2287 final double biasX = ba.getElementAtIndex(0);
2288 final double biasY = ba.getElementAtIndex(1);
2289 final double biasZ = ba.getElementAtIndex(2);
2290
2291 final Matrix ma = generateMaCommonAxis();
2292 final double sx = ma.getElementAt(0, 0);
2293 final double sy = ma.getElementAt(1, 1);
2294 final double sz = ma.getElementAt(2, 2);
2295
2296 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2297 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2298 biasX, biasY, biasZ, sx, sy, sz);
2299
2300
2301 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2302 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2303 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2304 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2305 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2306 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2307 final Acceleration bx2 = new Acceleration(0.0,
2308 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2309 calibrator.getBiasXAsAcceleration(bx2);
2310 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2311 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2312 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2313 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2314 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2315 final Acceleration by2 = new Acceleration(0.0,
2316 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2317 calibrator.getBiasYAsAcceleration(by2);
2318 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2319 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2320 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2321 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2322 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2323 final Acceleration bz2 = new Acceleration(0.0,
2324 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2325 calibrator.getBiasZAsAcceleration(bz2);
2326 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2327 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2328 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2329 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2330 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2331 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2332 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2333 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2334 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2335 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2336 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2337 final double[] bias1 = calibrator.getBias();
2338 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2339 final double[] bias2 = new double[3];
2340 calibrator.getBias(bias2);
2341 assertArrayEquals(bias1, bias2, 0.0);
2342 final Matrix b1 = calibrator.getBiasAsMatrix();
2343 assertEquals(b1, ba);
2344 final Matrix b2 = new Matrix(3, 1);
2345 calibrator.getBiasAsMatrix(b2);
2346 assertEquals(b1, b2);
2347 final Matrix ma1 = calibrator.getInitialMa();
2348 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2349 final Matrix ma2 = new Matrix(3, 3);
2350 calibrator.getInitialMa(ma2);
2351 assertEquals(ma1, ma2);
2352 assertSame(calibrator.getMeasurements(), measurements);
2353 assertFalse(calibrator.isCommonAxisUsed());
2354 assertNull(calibrator.getListener());
2355 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2356 assertFalse(calibrator.isReady());
2357 assertFalse(calibrator.isRunning());
2358 assertNull(calibrator.getEstimatedMa());
2359 assertNull(calibrator.getEstimatedSx());
2360 assertNull(calibrator.getEstimatedSy());
2361 assertNull(calibrator.getEstimatedSz());
2362 assertNull(calibrator.getEstimatedMxy());
2363 assertNull(calibrator.getEstimatedMxz());
2364 assertNull(calibrator.getEstimatedMyx());
2365 assertNull(calibrator.getEstimatedMyz());
2366 assertNull(calibrator.getEstimatedMzx());
2367 assertNull(calibrator.getEstimatedMzy());
2368 assertNull(calibrator.getEstimatedCovariance());
2369 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2370 assertNull(calibrator.getGroundTruthGravityNorm());
2371 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2372 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2373 }
2374
2375 @Test
2376 public void testConstructor27() throws WrongSizeException {
2377 final Collection<StandardDeviationBodyKinematics> measurements =
2378 Collections.emptyList();
2379
2380 final Matrix ba = generateBa();
2381 final double biasX = ba.getElementAtIndex(0);
2382 final double biasY = ba.getElementAtIndex(1);
2383 final double biasZ = ba.getElementAtIndex(2);
2384
2385 final Matrix ma = generateMaCommonAxis();
2386 final double sx = ma.getElementAt(0, 0);
2387 final double sy = ma.getElementAt(1, 1);
2388 final double sz = ma.getElementAt(2, 2);
2389
2390 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2391 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2392 biasX, biasY, biasZ, sx, sy, sz, this);
2393
2394
2395 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2396 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2397 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2398 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2399 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2400 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2401 final Acceleration bx2 = new Acceleration(0.0,
2402 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2403 calibrator.getBiasXAsAcceleration(bx2);
2404 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2405 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2407 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2408 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2409 final Acceleration by2 = new Acceleration(0.0,
2410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2411 calibrator.getBiasYAsAcceleration(by2);
2412 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2413 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2415 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2416 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2417 final Acceleration bz2 = new Acceleration(0.0,
2418 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2419 calibrator.getBiasZAsAcceleration(bz2);
2420 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2421 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2423 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2424 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2425 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2426 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2427 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2428 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2429 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2430 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2431 final double[] bias1 = calibrator.getBias();
2432 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2433 final double[] bias2 = new double[3];
2434 calibrator.getBias(bias2);
2435 assertArrayEquals(bias1, bias2, 0.0);
2436 final Matrix b1 = calibrator.getBiasAsMatrix();
2437 assertEquals(b1, ba);
2438 final Matrix b2 = new Matrix(3, 1);
2439 calibrator.getBiasAsMatrix(b2);
2440 assertEquals(b1, b2);
2441 final Matrix ma1 = calibrator.getInitialMa();
2442 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2443 final Matrix ma2 = new Matrix(3, 3);
2444 calibrator.getInitialMa(ma2);
2445 assertEquals(ma1, ma2);
2446 assertSame(calibrator.getMeasurements(), measurements);
2447 assertFalse(calibrator.isCommonAxisUsed());
2448 assertSame(calibrator.getListener(), this);
2449 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2450 assertFalse(calibrator.isReady());
2451 assertFalse(calibrator.isRunning());
2452 assertNull(calibrator.getEstimatedMa());
2453 assertNull(calibrator.getEstimatedSx());
2454 assertNull(calibrator.getEstimatedSy());
2455 assertNull(calibrator.getEstimatedSz());
2456 assertNull(calibrator.getEstimatedMxy());
2457 assertNull(calibrator.getEstimatedMxz());
2458 assertNull(calibrator.getEstimatedMyx());
2459 assertNull(calibrator.getEstimatedMyz());
2460 assertNull(calibrator.getEstimatedMzx());
2461 assertNull(calibrator.getEstimatedMzy());
2462 assertNull(calibrator.getEstimatedCovariance());
2463 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2464 assertNull(calibrator.getGroundTruthGravityNorm());
2465 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2466 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2467 }
2468
2469 @Test
2470 public void testConstructor28() throws WrongSizeException {
2471 final Matrix ba = generateBa();
2472 final double biasX = ba.getElementAtIndex(0);
2473 final double biasY = ba.getElementAtIndex(1);
2474 final double biasZ = ba.getElementAtIndex(2);
2475
2476 final Matrix ma = generateMaCommonAxis();
2477 final double sx = ma.getElementAt(0, 0);
2478 final double sy = ma.getElementAt(1, 1);
2479 final double sz = ma.getElementAt(2, 2);
2480
2481 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2482 new KnownBiasAndGravityNormAccelerometerCalibrator(
2483 true, biasX, biasY, biasZ, sx, sy, sz);
2484
2485
2486 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2487 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2488 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2489 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2490 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2491 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2492 final Acceleration bx2 = new Acceleration(0.0,
2493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2494 calibrator.getBiasXAsAcceleration(bx2);
2495 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2496 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2497 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2498 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2499 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2500 final Acceleration by2 = new Acceleration(0.0,
2501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2502 calibrator.getBiasYAsAcceleration(by2);
2503 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2504 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2506 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2507 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2508 final Acceleration bz2 = new Acceleration(0.0,
2509 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2510 calibrator.getBiasZAsAcceleration(bz2);
2511 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2512 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2513 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2514 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2515 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2516 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2517 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2518 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2519 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2520 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2521 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2522 final double[] bias1 = calibrator.getBias();
2523 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2524 final double[] bias2 = new double[3];
2525 calibrator.getBias(bias2);
2526 assertArrayEquals(bias1, bias2, 0.0);
2527 final Matrix b1 = calibrator.getBiasAsMatrix();
2528 assertEquals(b1, ba);
2529 final Matrix b2 = new Matrix(3, 1);
2530 calibrator.getBiasAsMatrix(b2);
2531 assertEquals(b1, b2);
2532 final Matrix ma1 = calibrator.getInitialMa();
2533 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2534 final Matrix ma2 = new Matrix(3, 3);
2535 calibrator.getInitialMa(ma2);
2536 assertEquals(ma1, ma2);
2537 assertNull(calibrator.getMeasurements());
2538 assertTrue(calibrator.isCommonAxisUsed());
2539 assertNull(calibrator.getListener());
2540 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2541 assertFalse(calibrator.isReady());
2542 assertFalse(calibrator.isRunning());
2543 assertNull(calibrator.getEstimatedMa());
2544 assertNull(calibrator.getEstimatedSx());
2545 assertNull(calibrator.getEstimatedSy());
2546 assertNull(calibrator.getEstimatedSz());
2547 assertNull(calibrator.getEstimatedMxy());
2548 assertNull(calibrator.getEstimatedMxz());
2549 assertNull(calibrator.getEstimatedMyx());
2550 assertNull(calibrator.getEstimatedMyz());
2551 assertNull(calibrator.getEstimatedMzx());
2552 assertNull(calibrator.getEstimatedMzy());
2553 assertNull(calibrator.getEstimatedCovariance());
2554 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2555 assertNull(calibrator.getGroundTruthGravityNorm());
2556 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2557 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2558 }
2559
2560 @Test
2561 public void testConstructor29() throws WrongSizeException {
2562 final Matrix ba = generateBa();
2563 final double biasX = ba.getElementAtIndex(0);
2564 final double biasY = ba.getElementAtIndex(1);
2565 final double biasZ = ba.getElementAtIndex(2);
2566
2567 final Matrix ma = generateMaCommonAxis();
2568 final double sx = ma.getElementAt(0, 0);
2569 final double sy = ma.getElementAt(1, 1);
2570 final double sz = ma.getElementAt(2, 2);
2571
2572 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2573 new KnownBiasAndGravityNormAccelerometerCalibrator(
2574 true, biasX, biasY, biasZ,
2575 sx, sy, sz, this);
2576
2577
2578 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2579 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2580 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2581 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2582 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2583 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2584 final Acceleration bx2 = new Acceleration(0.0,
2585 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2586 calibrator.getBiasXAsAcceleration(bx2);
2587 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2588 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2589 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2590 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2591 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2592 final Acceleration by2 = new Acceleration(0.0,
2593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2594 calibrator.getBiasYAsAcceleration(by2);
2595 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2596 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2597 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2598 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2599 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2600 final Acceleration bz2 = new Acceleration(0.0,
2601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2602 calibrator.getBiasZAsAcceleration(bz2);
2603 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2604 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2605 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2606 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2607 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2608 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2609 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2610 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2611 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2612 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2613 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2614 final double[] bias1 = calibrator.getBias();
2615 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2616 final double[] bias2 = new double[3];
2617 calibrator.getBias(bias2);
2618 assertArrayEquals(bias1, bias2, 0.0);
2619 final Matrix b1 = calibrator.getBiasAsMatrix();
2620 assertEquals(b1, ba);
2621 final Matrix b2 = new Matrix(3, 1);
2622 calibrator.getBiasAsMatrix(b2);
2623 assertEquals(b1, b2);
2624 final Matrix ma1 = calibrator.getInitialMa();
2625 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2626 final Matrix ma2 = new Matrix(3, 3);
2627 calibrator.getInitialMa(ma2);
2628 assertEquals(ma1, ma2);
2629 assertNull(calibrator.getMeasurements());
2630 assertTrue(calibrator.isCommonAxisUsed());
2631 assertSame(calibrator.getListener(), this);
2632 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2633 assertFalse(calibrator.isReady());
2634 assertFalse(calibrator.isRunning());
2635 assertNull(calibrator.getEstimatedMa());
2636 assertNull(calibrator.getEstimatedSx());
2637 assertNull(calibrator.getEstimatedSy());
2638 assertNull(calibrator.getEstimatedSz());
2639 assertNull(calibrator.getEstimatedMxy());
2640 assertNull(calibrator.getEstimatedMxz());
2641 assertNull(calibrator.getEstimatedMyx());
2642 assertNull(calibrator.getEstimatedMyz());
2643 assertNull(calibrator.getEstimatedMzx());
2644 assertNull(calibrator.getEstimatedMzy());
2645 assertNull(calibrator.getEstimatedCovariance());
2646 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2647 assertNull(calibrator.getGroundTruthGravityNorm());
2648 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2649 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2650 }
2651
2652 @Test
2653 public void testConstructor30() throws WrongSizeException {
2654 final Collection<StandardDeviationBodyKinematics> measurements =
2655 Collections.emptyList();
2656 final Matrix ba = generateBa();
2657 final double biasX = ba.getElementAtIndex(0);
2658 final double biasY = ba.getElementAtIndex(1);
2659 final double biasZ = ba.getElementAtIndex(2);
2660
2661 final Matrix ma = generateMaCommonAxis();
2662 final double sx = ma.getElementAt(0, 0);
2663 final double sy = ma.getElementAt(1, 1);
2664 final double sz = ma.getElementAt(2, 2);
2665
2666 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2667 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2668 true, biasX, biasY, biasZ,
2669 sx, sy, sz);
2670
2671
2672 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2673 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2674 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2675 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2676 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2677 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2678 final Acceleration bx2 = new Acceleration(0.0,
2679 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2680 calibrator.getBiasXAsAcceleration(bx2);
2681 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2682 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2683 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2684 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2685 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2686 final Acceleration by2 = new Acceleration(0.0,
2687 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2688 calibrator.getBiasYAsAcceleration(by2);
2689 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2690 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2691 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2692 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2693 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2694 final Acceleration bz2 = new Acceleration(0.0,
2695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2696 calibrator.getBiasZAsAcceleration(bz2);
2697 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2698 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2699 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2700 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2701 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2702 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2703 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2704 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2705 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2706 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2707 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2708 final double[] bias1 = calibrator.getBias();
2709 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2710 final double[] bias2 = new double[3];
2711 calibrator.getBias(bias2);
2712 assertArrayEquals(bias1, bias2, 0.0);
2713 final Matrix b1 = calibrator.getBiasAsMatrix();
2714 assertEquals(b1, ba);
2715 final Matrix b2 = new Matrix(3, 1);
2716 calibrator.getBiasAsMatrix(b2);
2717 assertEquals(b1, b2);
2718 final Matrix ma1 = calibrator.getInitialMa();
2719 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2720 final Matrix ma2 = new Matrix(3, 3);
2721 calibrator.getInitialMa(ma2);
2722 assertEquals(ma1, ma2);
2723 assertSame(calibrator.getMeasurements(), measurements);
2724 assertTrue(calibrator.isCommonAxisUsed());
2725 assertNull(calibrator.getListener());
2726 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2727 assertFalse(calibrator.isReady());
2728 assertFalse(calibrator.isRunning());
2729 assertNull(calibrator.getEstimatedMa());
2730 assertNull(calibrator.getEstimatedSx());
2731 assertNull(calibrator.getEstimatedSy());
2732 assertNull(calibrator.getEstimatedSz());
2733 assertNull(calibrator.getEstimatedMxy());
2734 assertNull(calibrator.getEstimatedMxz());
2735 assertNull(calibrator.getEstimatedMyx());
2736 assertNull(calibrator.getEstimatedMyz());
2737 assertNull(calibrator.getEstimatedMzx());
2738 assertNull(calibrator.getEstimatedMzy());
2739 assertNull(calibrator.getEstimatedCovariance());
2740 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2741 assertNull(calibrator.getGroundTruthGravityNorm());
2742 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2743 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2744 }
2745
2746 @Test
2747 public void testConstructor31() throws WrongSizeException {
2748 final Collection<StandardDeviationBodyKinematics> measurements =
2749 Collections.emptyList();
2750 final Matrix ba = generateBa();
2751 final double biasX = ba.getElementAtIndex(0);
2752 final double biasY = ba.getElementAtIndex(1);
2753 final double biasZ = ba.getElementAtIndex(2);
2754
2755 final Matrix ma = generateMaCommonAxis();
2756 final double sx = ma.getElementAt(0, 0);
2757 final double sy = ma.getElementAt(1, 1);
2758 final double sz = ma.getElementAt(2, 2);
2759
2760 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2761 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2762 true, biasX, biasY, biasZ,
2763 sx, sy, sz, this);
2764
2765
2766 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2767 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2768 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2769 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2770 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2771 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2772 final Acceleration bx2 = new Acceleration(0.0,
2773 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2774 calibrator.getBiasXAsAcceleration(bx2);
2775 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2776 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2777 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2778 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2779 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2780 final Acceleration by2 = new Acceleration(0.0,
2781 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2782 calibrator.getBiasYAsAcceleration(by2);
2783 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2784 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2785 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2786 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2787 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2788 final Acceleration bz2 = new Acceleration(0.0,
2789 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2790 calibrator.getBiasZAsAcceleration(bz2);
2791 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2792 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2793 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2794 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2795 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2796 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2797 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2798 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2799 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2800 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2801 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2802 final double[] bias1 = calibrator.getBias();
2803 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2804 final double[] bias2 = new double[3];
2805 calibrator.getBias(bias2);
2806 assertArrayEquals(bias1, bias2, 0.0);
2807 final Matrix b1 = calibrator.getBiasAsMatrix();
2808 assertEquals(b1, ba);
2809 final Matrix b2 = new Matrix(3, 1);
2810 calibrator.getBiasAsMatrix(b2);
2811 assertEquals(b1, b2);
2812 final Matrix ma1 = calibrator.getInitialMa();
2813 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2814 final Matrix ma2 = new Matrix(3, 3);
2815 calibrator.getInitialMa(ma2);
2816 assertEquals(ma1, ma2);
2817 assertSame(calibrator.getMeasurements(), measurements);
2818 assertTrue(calibrator.isCommonAxisUsed());
2819 assertSame(calibrator.getListener(), this);
2820 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2821 assertFalse(calibrator.isReady());
2822 assertFalse(calibrator.isRunning());
2823 assertNull(calibrator.getEstimatedMa());
2824 assertNull(calibrator.getEstimatedSx());
2825 assertNull(calibrator.getEstimatedSy());
2826 assertNull(calibrator.getEstimatedSz());
2827 assertNull(calibrator.getEstimatedMxy());
2828 assertNull(calibrator.getEstimatedMxz());
2829 assertNull(calibrator.getEstimatedMyx());
2830 assertNull(calibrator.getEstimatedMyz());
2831 assertNull(calibrator.getEstimatedMzx());
2832 assertNull(calibrator.getEstimatedMzy());
2833 assertNull(calibrator.getEstimatedCovariance());
2834 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2835 assertNull(calibrator.getGroundTruthGravityNorm());
2836 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2837 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2838 }
2839
2840 @Test
2841 public void testConstructor32() throws WrongSizeException {
2842 final Matrix ba = generateBa();
2843 final double biasX = ba.getElementAtIndex(0);
2844 final double biasY = ba.getElementAtIndex(1);
2845 final double biasZ = ba.getElementAtIndex(2);
2846
2847 final Matrix ma = generateMaCommonAxis();
2848 final double sx = ma.getElementAt(0, 0);
2849 final double sy = ma.getElementAt(1, 1);
2850 final double sz = ma.getElementAt(2, 2);
2851
2852 final Acceleration bx = new Acceleration(biasX,
2853 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2854 final Acceleration by = new Acceleration(biasY,
2855 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2856 final Acceleration bz = new Acceleration(biasZ,
2857 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2858
2859 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2860 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
2861 sx, sy, sz);
2862
2863
2864 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2865 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2866 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2867 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2868 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2869 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2870 final Acceleration bx2 = new Acceleration(0.0,
2871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2872 calibrator.getBiasXAsAcceleration(bx2);
2873 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2874 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2875 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2876 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2877 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2878 final Acceleration by2 = new Acceleration(0.0,
2879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2880 calibrator.getBiasYAsAcceleration(by2);
2881 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2882 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2883 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2884 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2885 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2886 final Acceleration bz2 = new Acceleration(0.0,
2887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2888 calibrator.getBiasZAsAcceleration(bz2);
2889 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2890 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2891 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2892 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2893 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2894 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2895 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2896 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2897 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2898 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2899 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2900 final double[] bias1 = calibrator.getBias();
2901 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2902 final double[] bias2 = new double[3];
2903 calibrator.getBias(bias2);
2904 assertArrayEquals(bias1, bias2, 0.0);
2905 final Matrix b1 = calibrator.getBiasAsMatrix();
2906 assertEquals(b1, ba);
2907 final Matrix b2 = new Matrix(3, 1);
2908 calibrator.getBiasAsMatrix(b2);
2909 assertEquals(b1, b2);
2910 final Matrix ma1 = calibrator.getInitialMa();
2911 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2912 final Matrix ma2 = new Matrix(3, 3);
2913 calibrator.getInitialMa(ma2);
2914 assertEquals(ma1, ma2);
2915 assertNull(calibrator.getMeasurements());
2916 assertFalse(calibrator.isCommonAxisUsed());
2917 assertNull(calibrator.getListener());
2918 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2919 assertFalse(calibrator.isReady());
2920 assertFalse(calibrator.isRunning());
2921 assertNull(calibrator.getEstimatedMa());
2922 assertNull(calibrator.getEstimatedSx());
2923 assertNull(calibrator.getEstimatedSy());
2924 assertNull(calibrator.getEstimatedSz());
2925 assertNull(calibrator.getEstimatedMxy());
2926 assertNull(calibrator.getEstimatedMxz());
2927 assertNull(calibrator.getEstimatedMyx());
2928 assertNull(calibrator.getEstimatedMyz());
2929 assertNull(calibrator.getEstimatedMzx());
2930 assertNull(calibrator.getEstimatedMzy());
2931 assertNull(calibrator.getEstimatedCovariance());
2932 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2933 assertNull(calibrator.getGroundTruthGravityNorm());
2934 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2935 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2936 }
2937
2938 @Test
2939 public void testConstructor33() throws WrongSizeException {
2940 final Matrix ba = generateBa();
2941 final double biasX = ba.getElementAtIndex(0);
2942 final double biasY = ba.getElementAtIndex(1);
2943 final double biasZ = ba.getElementAtIndex(2);
2944
2945 final Matrix ma = generateMaCommonAxis();
2946 final double sx = ma.getElementAt(0, 0);
2947 final double sy = ma.getElementAt(1, 1);
2948 final double sz = ma.getElementAt(2, 2);
2949
2950 final Acceleration bx = new Acceleration(biasX,
2951 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2952 final Acceleration by = new Acceleration(biasY,
2953 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2954 final Acceleration bz = new Acceleration(biasZ,
2955 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2956
2957 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2958 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
2959 sx, sy, sz, this);
2960
2961
2962 assertEquals(calibrator.getBiasX(), biasX, 0.0);
2963 assertEquals(calibrator.getBiasY(), biasY, 0.0);
2964 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2965 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2966 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2967 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2968 final Acceleration bx2 = new Acceleration(0.0,
2969 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2970 calibrator.getBiasXAsAcceleration(bx2);
2971 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2972 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2973 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2974 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2975 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2976 final Acceleration by2 = new Acceleration(0.0,
2977 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2978 calibrator.getBiasYAsAcceleration(by2);
2979 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2980 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2981 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2982 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2983 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2984 final Acceleration bz2 = new Acceleration(0.0,
2985 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2986 calibrator.getBiasZAsAcceleration(bz2);
2987 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2988 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2989 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2990 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2991 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2992 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2993 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2994 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2995 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2996 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2997 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2998 final double[] bias1 = calibrator.getBias();
2999 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3000 final double[] bias2 = new double[3];
3001 calibrator.getBias(bias2);
3002 assertArrayEquals(bias1, bias2, 0.0);
3003 final Matrix b1 = calibrator.getBiasAsMatrix();
3004 assertEquals(b1, ba);
3005 final Matrix b2 = new Matrix(3, 1);
3006 calibrator.getBiasAsMatrix(b2);
3007 assertEquals(b1, b2);
3008 final Matrix ma1 = calibrator.getInitialMa();
3009 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3010 final Matrix ma2 = new Matrix(3, 3);
3011 calibrator.getInitialMa(ma2);
3012 assertEquals(ma1, ma2);
3013 assertNull(calibrator.getMeasurements());
3014 assertFalse(calibrator.isCommonAxisUsed());
3015 assertSame(calibrator.getListener(), this);
3016 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3017 assertFalse(calibrator.isReady());
3018 assertFalse(calibrator.isRunning());
3019 assertNull(calibrator.getEstimatedMa());
3020 assertNull(calibrator.getEstimatedSx());
3021 assertNull(calibrator.getEstimatedSy());
3022 assertNull(calibrator.getEstimatedSz());
3023 assertNull(calibrator.getEstimatedMxy());
3024 assertNull(calibrator.getEstimatedMxz());
3025 assertNull(calibrator.getEstimatedMyx());
3026 assertNull(calibrator.getEstimatedMyz());
3027 assertNull(calibrator.getEstimatedMzx());
3028 assertNull(calibrator.getEstimatedMzy());
3029 assertNull(calibrator.getEstimatedCovariance());
3030 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3031 assertNull(calibrator.getGroundTruthGravityNorm());
3032 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3033 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3034 }
3035
3036 @Test
3037 public void testConstructor34() throws WrongSizeException {
3038 final Collection<StandardDeviationBodyKinematics> measurements =
3039 Collections.emptyList();
3040
3041 final Matrix ba = generateBa();
3042 final double biasX = ba.getElementAtIndex(0);
3043 final double biasY = ba.getElementAtIndex(1);
3044 final double biasZ = ba.getElementAtIndex(2);
3045
3046 final Matrix ma = generateMaCommonAxis();
3047 final double sx = ma.getElementAt(0, 0);
3048 final double sy = ma.getElementAt(1, 1);
3049 final double sz = ma.getElementAt(2, 2);
3050
3051 final Acceleration bx = new Acceleration(biasX,
3052 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3053 final Acceleration by = new Acceleration(biasY,
3054 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3055 final Acceleration bz = new Acceleration(biasZ,
3056 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3057
3058 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3059 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3060 bx, by, bz, sx, sy, sz);
3061
3062
3063 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3064 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3065 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3066 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3067 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3068 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3069 final Acceleration bx2 = new Acceleration(0.0,
3070 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3071 calibrator.getBiasXAsAcceleration(bx2);
3072 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3073 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3074 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3075 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3076 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3077 final Acceleration by2 = new Acceleration(0.0,
3078 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3079 calibrator.getBiasYAsAcceleration(by2);
3080 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3081 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3082 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3083 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3084 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3085 final Acceleration bz2 = new Acceleration(0.0,
3086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3087 calibrator.getBiasZAsAcceleration(bz2);
3088 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3089 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3090 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3091 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3092 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3093 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3094 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3095 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3096 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3097 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3098 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3099 final double[] bias1 = calibrator.getBias();
3100 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3101 final double[] bias2 = new double[3];
3102 calibrator.getBias(bias2);
3103 assertArrayEquals(bias1, bias2, 0.0);
3104 final Matrix b1 = calibrator.getBiasAsMatrix();
3105 assertEquals(b1, ba);
3106 final Matrix b2 = new Matrix(3, 1);
3107 calibrator.getBiasAsMatrix(b2);
3108 assertEquals(b1, b2);
3109 final Matrix ma1 = calibrator.getInitialMa();
3110 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3111 final Matrix ma2 = new Matrix(3, 3);
3112 calibrator.getInitialMa(ma2);
3113 assertEquals(ma1, ma2);
3114 assertSame(calibrator.getMeasurements(), measurements);
3115 assertFalse(calibrator.isCommonAxisUsed());
3116 assertNull(calibrator.getListener());
3117 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3118 assertFalse(calibrator.isReady());
3119 assertFalse(calibrator.isRunning());
3120 assertNull(calibrator.getEstimatedMa());
3121 assertNull(calibrator.getEstimatedSx());
3122 assertNull(calibrator.getEstimatedSy());
3123 assertNull(calibrator.getEstimatedSz());
3124 assertNull(calibrator.getEstimatedMxy());
3125 assertNull(calibrator.getEstimatedMxz());
3126 assertNull(calibrator.getEstimatedMyx());
3127 assertNull(calibrator.getEstimatedMyz());
3128 assertNull(calibrator.getEstimatedMzx());
3129 assertNull(calibrator.getEstimatedMzy());
3130 assertNull(calibrator.getEstimatedCovariance());
3131 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3132 assertNull(calibrator.getGroundTruthGravityNorm());
3133 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3134 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3135 }
3136
3137 @Test
3138 public void testConstructor35() throws WrongSizeException {
3139 final Collection<StandardDeviationBodyKinematics> measurements =
3140 Collections.emptyList();
3141
3142 final Matrix ba = generateBa();
3143 final double biasX = ba.getElementAtIndex(0);
3144 final double biasY = ba.getElementAtIndex(1);
3145 final double biasZ = ba.getElementAtIndex(2);
3146
3147 final Matrix ma = generateMaCommonAxis();
3148 final double sx = ma.getElementAt(0, 0);
3149 final double sy = ma.getElementAt(1, 1);
3150 final double sz = ma.getElementAt(2, 2);
3151
3152 final Acceleration bx = new Acceleration(biasX,
3153 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3154 final Acceleration by = new Acceleration(biasY,
3155 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156 final Acceleration bz = new Acceleration(biasZ,
3157 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3158
3159 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3160 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3161 bx, by, bz, sx, sy, sz, this);
3162
3163
3164 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3165 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3166 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3167 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3168 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3169 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3170 final Acceleration bx2 = new Acceleration(0.0,
3171 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3172 calibrator.getBiasXAsAcceleration(bx2);
3173 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3174 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3175 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3176 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3177 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3178 final Acceleration by2 = new Acceleration(0.0,
3179 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3180 calibrator.getBiasYAsAcceleration(by2);
3181 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3182 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3183 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3184 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3185 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3186 final Acceleration bz2 = new Acceleration(0.0,
3187 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3188 calibrator.getBiasZAsAcceleration(bz2);
3189 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3190 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3191 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3192 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3193 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3194 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3195 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3196 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3197 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3198 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3199 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3200 final double[] bias1 = calibrator.getBias();
3201 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3202 final double[] bias2 = new double[3];
3203 calibrator.getBias(bias2);
3204 assertArrayEquals(bias1, bias2, 0.0);
3205 final Matrix b1 = calibrator.getBiasAsMatrix();
3206 assertEquals(b1, ba);
3207 final Matrix b2 = new Matrix(3, 1);
3208 calibrator.getBiasAsMatrix(b2);
3209 assertEquals(b1, b2);
3210 final Matrix ma1 = calibrator.getInitialMa();
3211 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3212 final Matrix ma2 = new Matrix(3, 3);
3213 calibrator.getInitialMa(ma2);
3214 assertEquals(ma1, ma2);
3215 assertSame(calibrator.getMeasurements(), measurements);
3216 assertFalse(calibrator.isCommonAxisUsed());
3217 assertSame(calibrator.getListener(), this);
3218 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3219 assertFalse(calibrator.isReady());
3220 assertFalse(calibrator.isRunning());
3221 assertNull(calibrator.getEstimatedMa());
3222 assertNull(calibrator.getEstimatedSx());
3223 assertNull(calibrator.getEstimatedSy());
3224 assertNull(calibrator.getEstimatedSz());
3225 assertNull(calibrator.getEstimatedMxy());
3226 assertNull(calibrator.getEstimatedMxz());
3227 assertNull(calibrator.getEstimatedMyx());
3228 assertNull(calibrator.getEstimatedMyz());
3229 assertNull(calibrator.getEstimatedMzx());
3230 assertNull(calibrator.getEstimatedMzy());
3231 assertNull(calibrator.getEstimatedCovariance());
3232 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3233 assertNull(calibrator.getGroundTruthGravityNorm());
3234 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3235 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3236 }
3237
3238 @Test
3239 public void testConstructor36() throws WrongSizeException {
3240 final Matrix ba = generateBa();
3241 final double biasX = ba.getElementAtIndex(0);
3242 final double biasY = ba.getElementAtIndex(1);
3243 final double biasZ = ba.getElementAtIndex(2);
3244
3245 final Matrix ma = generateMaCommonAxis();
3246 final double sx = ma.getElementAt(0, 0);
3247 final double sy = ma.getElementAt(1, 1);
3248 final double sz = ma.getElementAt(2, 2);
3249
3250 final Acceleration bx = new Acceleration(biasX,
3251 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3252 final Acceleration by = new Acceleration(biasY,
3253 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3254 final Acceleration bz = new Acceleration(biasZ,
3255 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3256
3257 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3258 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
3259 bx, by, bz, sx, sy, sz);
3260
3261
3262 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3263 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3264 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3265 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3266 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3267 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3268 final Acceleration bx2 = new Acceleration(0.0,
3269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3270 calibrator.getBiasXAsAcceleration(bx2);
3271 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3272 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3273 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3274 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3275 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3276 final Acceleration by2 = new Acceleration(0.0,
3277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3278 calibrator.getBiasYAsAcceleration(by2);
3279 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3280 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3281 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3282 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3283 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3284 final Acceleration bz2 = new Acceleration(0.0,
3285 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3286 calibrator.getBiasZAsAcceleration(bz2);
3287 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3288 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3289 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3290 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3291 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3292 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3293 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3294 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3295 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3296 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3297 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3298 final double[] bias1 = calibrator.getBias();
3299 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3300 final double[] bias2 = new double[3];
3301 calibrator.getBias(bias2);
3302 assertArrayEquals(bias1, bias2, 0.0);
3303 final Matrix b1 = calibrator.getBiasAsMatrix();
3304 assertEquals(b1, ba);
3305 final Matrix b2 = new Matrix(3, 1);
3306 calibrator.getBiasAsMatrix(b2);
3307 assertEquals(b1, b2);
3308 final Matrix ma1 = calibrator.getInitialMa();
3309 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3310 final Matrix ma2 = new Matrix(3, 3);
3311 calibrator.getInitialMa(ma2);
3312 assertEquals(ma1, ma2);
3313 assertNull(calibrator.getMeasurements());
3314 assertTrue(calibrator.isCommonAxisUsed());
3315 assertNull(calibrator.getListener());
3316 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3317 assertFalse(calibrator.isReady());
3318 assertFalse(calibrator.isRunning());
3319 assertNull(calibrator.getEstimatedMa());
3320 assertNull(calibrator.getEstimatedSx());
3321 assertNull(calibrator.getEstimatedSy());
3322 assertNull(calibrator.getEstimatedSz());
3323 assertNull(calibrator.getEstimatedMxy());
3324 assertNull(calibrator.getEstimatedMxz());
3325 assertNull(calibrator.getEstimatedMyx());
3326 assertNull(calibrator.getEstimatedMyz());
3327 assertNull(calibrator.getEstimatedMzx());
3328 assertNull(calibrator.getEstimatedMzy());
3329 assertNull(calibrator.getEstimatedCovariance());
3330 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3331 assertNull(calibrator.getGroundTruthGravityNorm());
3332 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3333 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3334 }
3335
3336 @Test
3337 public void testConstructor37() throws WrongSizeException {
3338 final Matrix ba = generateBa();
3339 final double biasX = ba.getElementAtIndex(0);
3340 final double biasY = ba.getElementAtIndex(1);
3341 final double biasZ = ba.getElementAtIndex(2);
3342
3343 final Matrix ma = generateMaCommonAxis();
3344 final double sx = ma.getElementAt(0, 0);
3345 final double sy = ma.getElementAt(1, 1);
3346 final double sz = ma.getElementAt(2, 2);
3347
3348 final Acceleration bx = new Acceleration(biasX,
3349 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3350 final Acceleration by = new Acceleration(biasY,
3351 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3352 final Acceleration bz = new Acceleration(biasZ,
3353 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3354
3355 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3356 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
3357 bx, by, bz, sx, sy, sz, this);
3358
3359
3360 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3361 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3362 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3363 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3364 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3365 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3366 final Acceleration bx2 = new Acceleration(0.0,
3367 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3368 calibrator.getBiasXAsAcceleration(bx2);
3369 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3370 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3371 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3372 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3373 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3374 final Acceleration by2 = new Acceleration(0.0,
3375 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3376 calibrator.getBiasYAsAcceleration(by2);
3377 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3378 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3379 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3380 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3381 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3382 final Acceleration bz2 = new Acceleration(0.0,
3383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3384 calibrator.getBiasZAsAcceleration(bz2);
3385 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3386 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3387 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3388 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3389 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3390 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3391 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3392 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3393 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3394 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3395 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3396 final double[] bias1 = calibrator.getBias();
3397 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3398 final double[] bias2 = new double[3];
3399 calibrator.getBias(bias2);
3400 assertArrayEquals(bias1, bias2, 0.0);
3401 final Matrix b1 = calibrator.getBiasAsMatrix();
3402 assertEquals(b1, ba);
3403 final Matrix b2 = new Matrix(3, 1);
3404 calibrator.getBiasAsMatrix(b2);
3405 assertEquals(b1, b2);
3406 final Matrix ma1 = calibrator.getInitialMa();
3407 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3408 final Matrix ma2 = new Matrix(3, 3);
3409 calibrator.getInitialMa(ma2);
3410 assertEquals(ma1, ma2);
3411 assertNull(calibrator.getMeasurements());
3412 assertTrue(calibrator.isCommonAxisUsed());
3413 assertSame(calibrator.getListener(), this);
3414 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3415 assertFalse(calibrator.isReady());
3416 assertFalse(calibrator.isRunning());
3417 assertNull(calibrator.getEstimatedMa());
3418 assertNull(calibrator.getEstimatedSx());
3419 assertNull(calibrator.getEstimatedSy());
3420 assertNull(calibrator.getEstimatedSz());
3421 assertNull(calibrator.getEstimatedMxy());
3422 assertNull(calibrator.getEstimatedMxz());
3423 assertNull(calibrator.getEstimatedMyx());
3424 assertNull(calibrator.getEstimatedMyz());
3425 assertNull(calibrator.getEstimatedMzx());
3426 assertNull(calibrator.getEstimatedMzy());
3427 assertNull(calibrator.getEstimatedCovariance());
3428 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3429 assertNull(calibrator.getGroundTruthGravityNorm());
3430 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3431 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3432 }
3433
3434 @Test
3435 public void testConstructor38() throws WrongSizeException {
3436 final Collection<StandardDeviationBodyKinematics> measurements =
3437 Collections.emptyList();
3438
3439 final Matrix ba = generateBa();
3440 final double biasX = ba.getElementAtIndex(0);
3441 final double biasY = ba.getElementAtIndex(1);
3442 final double biasZ = ba.getElementAtIndex(2);
3443
3444 final Matrix ma = generateMaCommonAxis();
3445 final double sx = ma.getElementAt(0, 0);
3446 final double sy = ma.getElementAt(1, 1);
3447 final double sz = ma.getElementAt(2, 2);
3448
3449 final Acceleration bx = new Acceleration(biasX,
3450 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3451 final Acceleration by = new Acceleration(biasY,
3452 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3453 final Acceleration bz = new Acceleration(biasZ,
3454 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3455
3456 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3457 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3458 true, bx, by, bz, sx, sy, sz);
3459
3460
3461 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3462 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3463 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3464 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3465 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3466 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3467 final Acceleration bx2 = new Acceleration(0.0,
3468 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3469 calibrator.getBiasXAsAcceleration(bx2);
3470 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3471 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3472 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3473 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3474 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3475 final Acceleration by2 = new Acceleration(0.0,
3476 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3477 calibrator.getBiasYAsAcceleration(by2);
3478 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3479 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3480 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3481 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3482 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3483 final Acceleration bz2 = new Acceleration(0.0,
3484 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3485 calibrator.getBiasZAsAcceleration(bz2);
3486 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3487 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3488 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3489 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3490 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3491 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3492 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3493 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3494 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3495 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3496 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3497 final double[] bias1 = calibrator.getBias();
3498 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3499 final double[] bias2 = new double[3];
3500 calibrator.getBias(bias2);
3501 assertArrayEquals(bias1, bias2, 0.0);
3502 final Matrix b1 = calibrator.getBiasAsMatrix();
3503 assertEquals(b1, ba);
3504 final Matrix b2 = new Matrix(3, 1);
3505 calibrator.getBiasAsMatrix(b2);
3506 assertEquals(b1, b2);
3507 final Matrix ma1 = calibrator.getInitialMa();
3508 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3509 final Matrix ma2 = new Matrix(3, 3);
3510 calibrator.getInitialMa(ma2);
3511 assertEquals(ma1, ma2);
3512 assertSame(calibrator.getMeasurements(), measurements);
3513 assertTrue(calibrator.isCommonAxisUsed());
3514 assertNull(calibrator.getListener());
3515 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3516 assertFalse(calibrator.isReady());
3517 assertFalse(calibrator.isRunning());
3518 assertNull(calibrator.getEstimatedMa());
3519 assertNull(calibrator.getEstimatedSx());
3520 assertNull(calibrator.getEstimatedSy());
3521 assertNull(calibrator.getEstimatedSz());
3522 assertNull(calibrator.getEstimatedMxy());
3523 assertNull(calibrator.getEstimatedMxz());
3524 assertNull(calibrator.getEstimatedMyx());
3525 assertNull(calibrator.getEstimatedMyz());
3526 assertNull(calibrator.getEstimatedMzx());
3527 assertNull(calibrator.getEstimatedMzy());
3528 assertNull(calibrator.getEstimatedCovariance());
3529 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3530 assertNull(calibrator.getGroundTruthGravityNorm());
3531 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3532 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3533 }
3534
3535 @Test
3536 public void testConstructor39() throws WrongSizeException {
3537 final Collection<StandardDeviationBodyKinematics> measurements =
3538 Collections.emptyList();
3539
3540 final Matrix ba = generateBa();
3541 final double biasX = ba.getElementAtIndex(0);
3542 final double biasY = ba.getElementAtIndex(1);
3543 final double biasZ = ba.getElementAtIndex(2);
3544
3545 final Matrix ma = generateMaCommonAxis();
3546 final double sx = ma.getElementAt(0, 0);
3547 final double sy = ma.getElementAt(1, 1);
3548 final double sz = ma.getElementAt(2, 2);
3549
3550 final Acceleration bx = new Acceleration(biasX,
3551 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3552 final Acceleration by = new Acceleration(biasY,
3553 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3554 final Acceleration bz = new Acceleration(biasZ,
3555 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3556
3557 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3558 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3559 true, bx, by, bz, sx, sy, sz,
3560 this);
3561
3562
3563 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3564 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3565 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3566 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3567 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3568 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3569 final Acceleration bx2 = new Acceleration(0.0,
3570 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3571 calibrator.getBiasXAsAcceleration(bx2);
3572 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3573 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3574 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3575 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3576 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3577 final Acceleration by2 = new Acceleration(0.0,
3578 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3579 calibrator.getBiasYAsAcceleration(by2);
3580 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3581 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3583 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3584 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3585 final Acceleration bz2 = new Acceleration(0.0,
3586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3587 calibrator.getBiasZAsAcceleration(bz2);
3588 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3589 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3590 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3591 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3592 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3593 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3594 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3595 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3596 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3597 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3598 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3599 final double[] bias1 = calibrator.getBias();
3600 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3601 final double[] bias2 = new double[3];
3602 calibrator.getBias(bias2);
3603 assertArrayEquals(bias1, bias2, 0.0);
3604 final Matrix b1 = calibrator.getBiasAsMatrix();
3605 assertEquals(b1, ba);
3606 final Matrix b2 = new Matrix(3, 1);
3607 calibrator.getBiasAsMatrix(b2);
3608 assertEquals(b1, b2);
3609 final Matrix ma1 = calibrator.getInitialMa();
3610 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3611 final Matrix ma2 = new Matrix(3, 3);
3612 calibrator.getInitialMa(ma2);
3613 assertEquals(ma1, ma2);
3614 assertSame(calibrator.getMeasurements(), measurements);
3615 assertTrue(calibrator.isCommonAxisUsed());
3616 assertSame(calibrator.getListener(), this);
3617 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3618 assertFalse(calibrator.isReady());
3619 assertFalse(calibrator.isRunning());
3620 assertNull(calibrator.getEstimatedMa());
3621 assertNull(calibrator.getEstimatedSx());
3622 assertNull(calibrator.getEstimatedSy());
3623 assertNull(calibrator.getEstimatedSz());
3624 assertNull(calibrator.getEstimatedMxy());
3625 assertNull(calibrator.getEstimatedMxz());
3626 assertNull(calibrator.getEstimatedMyx());
3627 assertNull(calibrator.getEstimatedMyz());
3628 assertNull(calibrator.getEstimatedMzx());
3629 assertNull(calibrator.getEstimatedMzy());
3630 assertNull(calibrator.getEstimatedCovariance());
3631 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3632 assertNull(calibrator.getGroundTruthGravityNorm());
3633 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3634 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3635 }
3636
3637 @Test
3638 public void testConstructor40() throws WrongSizeException {
3639 final Matrix ba = generateBa();
3640 final double biasX = ba.getElementAtIndex(0);
3641 final double biasY = ba.getElementAtIndex(1);
3642 final double biasZ = ba.getElementAtIndex(2);
3643
3644 final Matrix ma = generateMaCommonAxis();
3645 final double sx = ma.getElementAt(0, 0);
3646 final double sy = ma.getElementAt(1, 1);
3647 final double sz = ma.getElementAt(2, 2);
3648 final double mxy = ma.getElementAt(0, 1);
3649 final double mxz = ma.getElementAt(0, 2);
3650 final double myx = ma.getElementAt(1, 0);
3651 final double myz = ma.getElementAt(1, 2);
3652 final double mzx = ma.getElementAt(2, 0);
3653 final double mzy = ma.getElementAt(2, 1);
3654
3655 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3656 new KnownBiasAndGravityNormAccelerometerCalibrator(
3657 biasX, biasY, biasZ, sx, sy, sz,
3658 mxy, mxz, myx, myz, mzx, mzy);
3659
3660
3661 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3662 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3663 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3664 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3665 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3666 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3667 final Acceleration bx2 = new Acceleration(0.0,
3668 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3669 calibrator.getBiasXAsAcceleration(bx2);
3670 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3671 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3672 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3673 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3674 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3675 final Acceleration by2 = new Acceleration(0.0,
3676 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3677 calibrator.getBiasYAsAcceleration(by2);
3678 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3679 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3680 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3681 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3682 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3683 final Acceleration bz2 = new Acceleration(0.0,
3684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3685 calibrator.getBiasZAsAcceleration(bz2);
3686 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3687 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3688 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3689 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3690 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3691 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3692 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3693 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3694 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3695 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3696 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3697 final double[] bias1 = calibrator.getBias();
3698 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3699 final double[] bias2 = new double[3];
3700 calibrator.getBias(bias2);
3701 assertArrayEquals(bias1, bias2, 0.0);
3702 final Matrix b1 = calibrator.getBiasAsMatrix();
3703 assertEquals(b1, ba);
3704 final Matrix b2 = new Matrix(3, 1);
3705 calibrator.getBiasAsMatrix(b2);
3706 assertEquals(b1, b2);
3707 final Matrix ma1 = new Matrix(3, 3);
3708 ma1.setSubmatrix(0, 0,
3709 2, 2,
3710 new double[]{sx, myx, mzx,
3711 mxy, sy, mzy,
3712 mxz, myz, sz});
3713 assertEquals(calibrator.getInitialMa(), ma1);
3714 final Matrix ma2 = new Matrix(3, 3);
3715 calibrator.getInitialMa(ma2);
3716 assertEquals(ma1, ma2);
3717 assertNull(calibrator.getMeasurements());
3718 assertFalse(calibrator.isCommonAxisUsed());
3719 assertNull(calibrator.getListener());
3720 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3721 assertFalse(calibrator.isReady());
3722 assertFalse(calibrator.isRunning());
3723 assertNull(calibrator.getEstimatedMa());
3724 assertNull(calibrator.getEstimatedSx());
3725 assertNull(calibrator.getEstimatedSy());
3726 assertNull(calibrator.getEstimatedSz());
3727 assertNull(calibrator.getEstimatedMxy());
3728 assertNull(calibrator.getEstimatedMxz());
3729 assertNull(calibrator.getEstimatedMyx());
3730 assertNull(calibrator.getEstimatedMyz());
3731 assertNull(calibrator.getEstimatedMzx());
3732 assertNull(calibrator.getEstimatedMzy());
3733 assertNull(calibrator.getEstimatedCovariance());
3734 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3735 assertNull(calibrator.getGroundTruthGravityNorm());
3736 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3737 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3738 }
3739
3740 @Test
3741 public void testConstructor41() throws WrongSizeException {
3742 final Collection<StandardDeviationBodyKinematics> measurements =
3743 Collections.emptyList();
3744
3745 final Matrix ba = generateBa();
3746 final double biasX = ba.getElementAtIndex(0);
3747 final double biasY = ba.getElementAtIndex(1);
3748 final double biasZ = ba.getElementAtIndex(2);
3749
3750 final Matrix ma = generateMaCommonAxis();
3751 final double sx = ma.getElementAt(0, 0);
3752 final double sy = ma.getElementAt(1, 1);
3753 final double sz = ma.getElementAt(2, 2);
3754 final double mxy = ma.getElementAt(0, 1);
3755 final double mxz = ma.getElementAt(0, 2);
3756 final double myx = ma.getElementAt(1, 0);
3757 final double myz = ma.getElementAt(1, 2);
3758 final double mzx = ma.getElementAt(2, 0);
3759 final double mzy = ma.getElementAt(2, 1);
3760
3761 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3762 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3763 biasX, biasY, biasZ, sx, sy, sz,
3764 mxy, mxz, myx, myz, mzx, mzy);
3765
3766
3767 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3768 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3769 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3770 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3771 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3772 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3773 final Acceleration bx2 = new Acceleration(0.0,
3774 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3775 calibrator.getBiasXAsAcceleration(bx2);
3776 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3777 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3778 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3779 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3780 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3781 final Acceleration by2 = new Acceleration(0.0,
3782 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3783 calibrator.getBiasYAsAcceleration(by2);
3784 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3785 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3786 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3787 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3788 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3789 final Acceleration bz2 = new Acceleration(0.0,
3790 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3791 calibrator.getBiasZAsAcceleration(bz2);
3792 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3793 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3794 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3795 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3796 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3797 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3798 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3799 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3800 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3801 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3802 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3803 final double[] bias1 = calibrator.getBias();
3804 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3805 final double[] bias2 = new double[3];
3806 calibrator.getBias(bias2);
3807 assertArrayEquals(bias1, bias2, 0.0);
3808 final Matrix b1 = calibrator.getBiasAsMatrix();
3809 assertEquals(b1, ba);
3810 final Matrix b2 = new Matrix(3, 1);
3811 calibrator.getBiasAsMatrix(b2);
3812 assertEquals(b1, b2);
3813 final Matrix ma1 = new Matrix(3, 3);
3814 ma1.setSubmatrix(0, 0,
3815 2, 2,
3816 new double[]{sx, myx, mzx,
3817 mxy, sy, mzy,
3818 mxz, myz, sz});
3819 assertEquals(calibrator.getInitialMa(), ma1);
3820 final Matrix ma2 = new Matrix(3, 3);
3821 calibrator.getInitialMa(ma2);
3822 assertEquals(ma1, ma2);
3823 assertSame(calibrator.getMeasurements(), measurements);
3824 assertFalse(calibrator.isCommonAxisUsed());
3825 assertNull(calibrator.getListener());
3826 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3827 assertFalse(calibrator.isReady());
3828 assertFalse(calibrator.isRunning());
3829 assertNull(calibrator.getEstimatedMa());
3830 assertNull(calibrator.getEstimatedSx());
3831 assertNull(calibrator.getEstimatedSy());
3832 assertNull(calibrator.getEstimatedSz());
3833 assertNull(calibrator.getEstimatedMxy());
3834 assertNull(calibrator.getEstimatedMxz());
3835 assertNull(calibrator.getEstimatedMyx());
3836 assertNull(calibrator.getEstimatedMyz());
3837 assertNull(calibrator.getEstimatedMzx());
3838 assertNull(calibrator.getEstimatedMzy());
3839 assertNull(calibrator.getEstimatedCovariance());
3840 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3841 assertNull(calibrator.getGroundTruthGravityNorm());
3842 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3843 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3844 }
3845
3846 @Test
3847 public void testConstructor42() throws WrongSizeException {
3848 final Collection<StandardDeviationBodyKinematics> measurements =
3849 Collections.emptyList();
3850
3851 final Matrix ba = generateBa();
3852 final double biasX = ba.getElementAtIndex(0);
3853 final double biasY = ba.getElementAtIndex(1);
3854 final double biasZ = ba.getElementAtIndex(2);
3855
3856 final Matrix ma = generateMaCommonAxis();
3857 final double sx = ma.getElementAt(0, 0);
3858 final double sy = ma.getElementAt(1, 1);
3859 final double sz = ma.getElementAt(2, 2);
3860 final double mxy = ma.getElementAt(0, 1);
3861 final double mxz = ma.getElementAt(0, 2);
3862 final double myx = ma.getElementAt(1, 0);
3863 final double myz = ma.getElementAt(1, 2);
3864 final double mzx = ma.getElementAt(2, 0);
3865 final double mzy = ma.getElementAt(2, 1);
3866
3867 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3868 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3869 biasX, biasY, biasZ, sx, sy, sz,
3870 mxy, mxz, myx, myz, mzx, mzy, this);
3871
3872
3873 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3874 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3875 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3876 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3877 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3878 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3879 final Acceleration bx2 = new Acceleration(0.0,
3880 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3881 calibrator.getBiasXAsAcceleration(bx2);
3882 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3883 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3884 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3885 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3886 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3887 final Acceleration by2 = new Acceleration(0.0,
3888 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3889 calibrator.getBiasYAsAcceleration(by2);
3890 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3891 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3892 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3893 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3894 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3895 final Acceleration bz2 = new Acceleration(0.0,
3896 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3897 calibrator.getBiasZAsAcceleration(bz2);
3898 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3899 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3900 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3901 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3902 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3903 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3904 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3905 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3906 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3907 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3908 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3909 final double[] bias1 = calibrator.getBias();
3910 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3911 final double[] bias2 = new double[3];
3912 calibrator.getBias(bias2);
3913 assertArrayEquals(bias1, bias2, 0.0);
3914 final Matrix b1 = calibrator.getBiasAsMatrix();
3915 assertEquals(b1, ba);
3916 final Matrix b2 = new Matrix(3, 1);
3917 calibrator.getBiasAsMatrix(b2);
3918 assertEquals(b1, b2);
3919 final Matrix ma1 = new Matrix(3, 3);
3920 ma1.setSubmatrix(0, 0,
3921 2, 2,
3922 new double[]{sx, myx, mzx,
3923 mxy, sy, mzy,
3924 mxz, myz, sz});
3925 assertEquals(calibrator.getInitialMa(), ma1);
3926 final Matrix ma2 = new Matrix(3, 3);
3927 calibrator.getInitialMa(ma2);
3928 assertEquals(ma1, ma2);
3929 assertSame(calibrator.getMeasurements(), measurements);
3930 assertFalse(calibrator.isCommonAxisUsed());
3931 assertSame(calibrator.getListener(), this);
3932 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3933 assertFalse(calibrator.isReady());
3934 assertFalse(calibrator.isRunning());
3935 assertNull(calibrator.getEstimatedMa());
3936 assertNull(calibrator.getEstimatedSx());
3937 assertNull(calibrator.getEstimatedSy());
3938 assertNull(calibrator.getEstimatedSz());
3939 assertNull(calibrator.getEstimatedMxy());
3940 assertNull(calibrator.getEstimatedMxz());
3941 assertNull(calibrator.getEstimatedMyx());
3942 assertNull(calibrator.getEstimatedMyz());
3943 assertNull(calibrator.getEstimatedMzx());
3944 assertNull(calibrator.getEstimatedMzy());
3945 assertNull(calibrator.getEstimatedCovariance());
3946 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3947 assertNull(calibrator.getGroundTruthGravityNorm());
3948 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3949 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3950 }
3951
3952 @Test
3953 public void testConstructor43() throws WrongSizeException {
3954 final Matrix ba = generateBa();
3955 final double biasX = ba.getElementAtIndex(0);
3956 final double biasY = ba.getElementAtIndex(1);
3957 final double biasZ = ba.getElementAtIndex(2);
3958
3959 final Matrix ma = generateMaCommonAxis();
3960 final double sx = ma.getElementAt(0, 0);
3961 final double sy = ma.getElementAt(1, 1);
3962 final double sz = ma.getElementAt(2, 2);
3963 final double mxy = ma.getElementAt(0, 1);
3964 final double mxz = ma.getElementAt(0, 2);
3965 final double myx = ma.getElementAt(1, 0);
3966 final double myz = ma.getElementAt(1, 2);
3967 final double mzx = ma.getElementAt(2, 0);
3968 final double mzy = ma.getElementAt(2, 1);
3969
3970 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3971 new KnownBiasAndGravityNormAccelerometerCalibrator(
3972 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
3973 myz, mzx, mzy);
3974
3975
3976 assertEquals(calibrator.getBiasX(), biasX, 0.0);
3977 assertEquals(calibrator.getBiasY(), biasY, 0.0);
3978 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3979 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3980 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3981 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3982 final Acceleration bx2 = new Acceleration(0.0,
3983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3984 calibrator.getBiasXAsAcceleration(bx2);
3985 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3986 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3987 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3988 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3989 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3990 final Acceleration by2 = new Acceleration(0.0,
3991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3992 calibrator.getBiasYAsAcceleration(by2);
3993 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3994 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3995 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3996 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3997 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3998 final Acceleration bz2 = new Acceleration(0.0,
3999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4000 calibrator.getBiasZAsAcceleration(bz2);
4001 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4002 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4003 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4004 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4005 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4006 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4007 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4008 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4009 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4010 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4011 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4012 final double[] bias1 = calibrator.getBias();
4013 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4014 final double[] bias2 = new double[3];
4015 calibrator.getBias(bias2);
4016 assertArrayEquals(bias1, bias2, 0.0);
4017 final Matrix b1 = calibrator.getBiasAsMatrix();
4018 assertEquals(b1, ba);
4019 final Matrix b2 = new Matrix(3, 1);
4020 calibrator.getBiasAsMatrix(b2);
4021 assertEquals(b1, b2);
4022 final Matrix ma1 = new Matrix(3, 3);
4023 ma1.setSubmatrix(0, 0,
4024 2, 2,
4025 new double[]{sx, myx, mzx,
4026 mxy, sy, mzy,
4027 mxz, myz, sz});
4028 assertEquals(calibrator.getInitialMa(), ma1);
4029 final Matrix ma2 = new Matrix(3, 3);
4030 calibrator.getInitialMa(ma2);
4031 assertEquals(ma1, ma2);
4032 assertNull(calibrator.getMeasurements());
4033 assertTrue(calibrator.isCommonAxisUsed());
4034 assertNull(calibrator.getListener());
4035 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4036 assertFalse(calibrator.isReady());
4037 assertFalse(calibrator.isRunning());
4038 assertNull(calibrator.getEstimatedMa());
4039 assertNull(calibrator.getEstimatedSx());
4040 assertNull(calibrator.getEstimatedSy());
4041 assertNull(calibrator.getEstimatedSz());
4042 assertNull(calibrator.getEstimatedMxy());
4043 assertNull(calibrator.getEstimatedMxz());
4044 assertNull(calibrator.getEstimatedMyx());
4045 assertNull(calibrator.getEstimatedMyz());
4046 assertNull(calibrator.getEstimatedMzx());
4047 assertNull(calibrator.getEstimatedMzy());
4048 assertNull(calibrator.getEstimatedCovariance());
4049 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4050 assertNull(calibrator.getGroundTruthGravityNorm());
4051 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4052 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4053 }
4054
4055 @Test
4056 public void testConstructor44() throws WrongSizeException {
4057 final Matrix ba = generateBa();
4058 final double biasX = ba.getElementAtIndex(0);
4059 final double biasY = ba.getElementAtIndex(1);
4060 final double biasZ = ba.getElementAtIndex(2);
4061
4062 final Matrix ma = generateMaCommonAxis();
4063 final double sx = ma.getElementAt(0, 0);
4064 final double sy = ma.getElementAt(1, 1);
4065 final double sz = ma.getElementAt(2, 2);
4066 final double mxy = ma.getElementAt(0, 1);
4067 final double mxz = ma.getElementAt(0, 2);
4068 final double myx = ma.getElementAt(1, 0);
4069 final double myz = ma.getElementAt(1, 2);
4070 final double mzx = ma.getElementAt(2, 0);
4071 final double mzy = ma.getElementAt(2, 1);
4072
4073 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4074 new KnownBiasAndGravityNormAccelerometerCalibrator(
4075 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4076 myz, mzx, mzy, this);
4077
4078
4079 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4080 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4081 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4082 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4083 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4084 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4085 final Acceleration bx2 = new Acceleration(0.0,
4086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4087 calibrator.getBiasXAsAcceleration(bx2);
4088 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4089 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4090 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4091 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4092 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4093 final Acceleration by2 = new Acceleration(0.0,
4094 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4095 calibrator.getBiasYAsAcceleration(by2);
4096 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4097 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4098 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4099 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4100 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4101 final Acceleration bz2 = new Acceleration(0.0,
4102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4103 calibrator.getBiasZAsAcceleration(bz2);
4104 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4105 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4106 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4107 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4108 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4109 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4110 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4111 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4112 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4113 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4114 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4115 final double[] bias1 = calibrator.getBias();
4116 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4117 final double[] bias2 = new double[3];
4118 calibrator.getBias(bias2);
4119 assertArrayEquals(bias1, bias2, 0.0);
4120 final Matrix b1 = calibrator.getBiasAsMatrix();
4121 assertEquals(b1, ba);
4122 final Matrix b2 = new Matrix(3, 1);
4123 calibrator.getBiasAsMatrix(b2);
4124 assertEquals(b1, b2);
4125 final Matrix ma1 = new Matrix(3, 3);
4126 ma1.setSubmatrix(0, 0,
4127 2, 2,
4128 new double[]{sx, myx, mzx,
4129 mxy, sy, mzy,
4130 mxz, myz, sz});
4131 assertEquals(calibrator.getInitialMa(), ma1);
4132 final Matrix ma2 = new Matrix(3, 3);
4133 calibrator.getInitialMa(ma2);
4134 assertEquals(ma1, ma2);
4135 assertNull(calibrator.getMeasurements());
4136 assertTrue(calibrator.isCommonAxisUsed());
4137 assertSame(calibrator.getListener(), this);
4138 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4139 assertFalse(calibrator.isReady());
4140 assertFalse(calibrator.isRunning());
4141 assertNull(calibrator.getEstimatedMa());
4142 assertNull(calibrator.getEstimatedSx());
4143 assertNull(calibrator.getEstimatedSy());
4144 assertNull(calibrator.getEstimatedSz());
4145 assertNull(calibrator.getEstimatedMxy());
4146 assertNull(calibrator.getEstimatedMxz());
4147 assertNull(calibrator.getEstimatedMyx());
4148 assertNull(calibrator.getEstimatedMyz());
4149 assertNull(calibrator.getEstimatedMzx());
4150 assertNull(calibrator.getEstimatedMzy());
4151 assertNull(calibrator.getEstimatedCovariance());
4152 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4153 assertNull(calibrator.getGroundTruthGravityNorm());
4154 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4155 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4156 }
4157
4158 @Test
4159 public void testConstructor45() throws WrongSizeException {
4160 final Collection<StandardDeviationBodyKinematics> measurements =
4161 Collections.emptyList();
4162
4163 final Matrix ba = generateBa();
4164 final double biasX = ba.getElementAtIndex(0);
4165 final double biasY = ba.getElementAtIndex(1);
4166 final double biasZ = ba.getElementAtIndex(2);
4167
4168 final Matrix ma = generateMaCommonAxis();
4169 final double sx = ma.getElementAt(0, 0);
4170 final double sy = ma.getElementAt(1, 1);
4171 final double sz = ma.getElementAt(2, 2);
4172 final double mxy = ma.getElementAt(0, 1);
4173 final double mxz = ma.getElementAt(0, 2);
4174 final double myx = ma.getElementAt(1, 0);
4175 final double myz = ma.getElementAt(1, 2);
4176 final double mzx = ma.getElementAt(2, 0);
4177 final double mzy = ma.getElementAt(2, 1);
4178
4179 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4180 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
4181 true, biasX, biasY, biasZ,
4182 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4183
4184
4185 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4186 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4187 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4188 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4189 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4190 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4191 final Acceleration bx2 = new Acceleration(0.0,
4192 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4193 calibrator.getBiasXAsAcceleration(bx2);
4194 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4195 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4196 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4197 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4198 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4199 final Acceleration by2 = new Acceleration(0.0,
4200 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4201 calibrator.getBiasYAsAcceleration(by2);
4202 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4203 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4204 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4205 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4206 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4207 final Acceleration bz2 = new Acceleration(0.0,
4208 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4209 calibrator.getBiasZAsAcceleration(bz2);
4210 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4211 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4212 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4213 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4214 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4215 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4216 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4217 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4218 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4219 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4220 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4221 final double[] bias1 = calibrator.getBias();
4222 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4223 final double[] bias2 = new double[3];
4224 calibrator.getBias(bias2);
4225 assertArrayEquals(bias1, bias2, 0.0);
4226 final Matrix b1 = calibrator.getBiasAsMatrix();
4227 assertEquals(b1, ba);
4228 final Matrix b2 = new Matrix(3, 1);
4229 calibrator.getBiasAsMatrix(b2);
4230 assertEquals(b1, b2);
4231 final Matrix ma1 = new Matrix(3, 3);
4232 ma1.setSubmatrix(0, 0,
4233 2, 2,
4234 new double[]{sx, myx, mzx,
4235 mxy, sy, mzy,
4236 mxz, myz, sz});
4237 assertEquals(calibrator.getInitialMa(), ma1);
4238 final Matrix ma2 = new Matrix(3, 3);
4239 calibrator.getInitialMa(ma2);
4240 assertEquals(ma1, ma2);
4241 assertSame(calibrator.getMeasurements(), measurements);
4242 assertTrue(calibrator.isCommonAxisUsed());
4243 assertNull(calibrator.getListener());
4244 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4245 assertFalse(calibrator.isReady());
4246 assertFalse(calibrator.isRunning());
4247 assertNull(calibrator.getEstimatedMa());
4248 assertNull(calibrator.getEstimatedSx());
4249 assertNull(calibrator.getEstimatedSy());
4250 assertNull(calibrator.getEstimatedSz());
4251 assertNull(calibrator.getEstimatedMxy());
4252 assertNull(calibrator.getEstimatedMxz());
4253 assertNull(calibrator.getEstimatedMyx());
4254 assertNull(calibrator.getEstimatedMyz());
4255 assertNull(calibrator.getEstimatedMzx());
4256 assertNull(calibrator.getEstimatedMzy());
4257 assertNull(calibrator.getEstimatedCovariance());
4258 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4259 assertNull(calibrator.getGroundTruthGravityNorm());
4260 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4261 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4262 }
4263
4264 @Test
4265 public void testConstructor46() throws WrongSizeException {
4266 final Collection<StandardDeviationBodyKinematics> measurements =
4267 Collections.emptyList();
4268
4269 final Matrix ba = generateBa();
4270 final double biasX = ba.getElementAtIndex(0);
4271 final double biasY = ba.getElementAtIndex(1);
4272 final double biasZ = ba.getElementAtIndex(2);
4273
4274 final Matrix ma = generateMaCommonAxis();
4275 final double sx = ma.getElementAt(0, 0);
4276 final double sy = ma.getElementAt(1, 1);
4277 final double sz = ma.getElementAt(2, 2);
4278 final double mxy = ma.getElementAt(0, 1);
4279 final double mxz = ma.getElementAt(0, 2);
4280 final double myx = ma.getElementAt(1, 0);
4281 final double myz = ma.getElementAt(1, 2);
4282 final double mzx = ma.getElementAt(2, 0);
4283 final double mzy = ma.getElementAt(2, 1);
4284
4285 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4286 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
4287 true, biasX, biasY, biasZ,
4288 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4289
4290
4291 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4292 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4293 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4294 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4295 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4296 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4297 final Acceleration bx2 = new Acceleration(0.0,
4298 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4299 calibrator.getBiasXAsAcceleration(bx2);
4300 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4301 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4302 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4303 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4304 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4305 final Acceleration by2 = new Acceleration(0.0,
4306 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4307 calibrator.getBiasYAsAcceleration(by2);
4308 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4309 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4310 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4311 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4312 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4313 final Acceleration bz2 = new Acceleration(0.0,
4314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4315 calibrator.getBiasZAsAcceleration(bz2);
4316 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4317 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4318 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4319 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4320 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4321 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4322 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4323 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4324 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4325 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4326 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4327 final double[] bias1 = calibrator.getBias();
4328 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4329 final double[] bias2 = new double[3];
4330 calibrator.getBias(bias2);
4331 assertArrayEquals(bias1, bias2, 0.0);
4332 final Matrix b1 = calibrator.getBiasAsMatrix();
4333 assertEquals(b1, ba);
4334 final Matrix b2 = new Matrix(3, 1);
4335 calibrator.getBiasAsMatrix(b2);
4336 assertEquals(b1, b2);
4337 final Matrix ma1 = new Matrix(3, 3);
4338 ma1.setSubmatrix(0, 0,
4339 2, 2,
4340 new double[]{sx, myx, mzx,
4341 mxy, sy, mzy,
4342 mxz, myz, sz});
4343 assertEquals(calibrator.getInitialMa(), ma1);
4344 final Matrix ma2 = new Matrix(3, 3);
4345 calibrator.getInitialMa(ma2);
4346 assertEquals(ma1, ma2);
4347 assertSame(calibrator.getMeasurements(), measurements);
4348 assertTrue(calibrator.isCommonAxisUsed());
4349 assertSame(calibrator.getListener(), this);
4350 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4351 assertFalse(calibrator.isReady());
4352 assertFalse(calibrator.isRunning());
4353 assertNull(calibrator.getEstimatedMa());
4354 assertNull(calibrator.getEstimatedSx());
4355 assertNull(calibrator.getEstimatedSy());
4356 assertNull(calibrator.getEstimatedSz());
4357 assertNull(calibrator.getEstimatedMxy());
4358 assertNull(calibrator.getEstimatedMxz());
4359 assertNull(calibrator.getEstimatedMyx());
4360 assertNull(calibrator.getEstimatedMyz());
4361 assertNull(calibrator.getEstimatedMzx());
4362 assertNull(calibrator.getEstimatedMzy());
4363 assertNull(calibrator.getEstimatedCovariance());
4364 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4365 assertNull(calibrator.getGroundTruthGravityNorm());
4366 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4367 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4368 }
4369
4370 @Test
4371 public void testConstructor47() throws WrongSizeException {
4372 final Matrix ba = generateBa();
4373 final double biasX = ba.getElementAtIndex(0);
4374 final double biasY = ba.getElementAtIndex(1);
4375 final double biasZ = ba.getElementAtIndex(2);
4376
4377 final Matrix ma = generateMaCommonAxis();
4378 final double sx = ma.getElementAt(0, 0);
4379 final double sy = ma.getElementAt(1, 1);
4380 final double sz = ma.getElementAt(2, 2);
4381 final double mxy = ma.getElementAt(0, 1);
4382 final double mxz = ma.getElementAt(0, 2);
4383 final double myx = ma.getElementAt(1, 0);
4384 final double myz = ma.getElementAt(1, 2);
4385 final double mzx = ma.getElementAt(2, 0);
4386 final double mzy = ma.getElementAt(2, 1);
4387
4388 final Acceleration bx = new Acceleration(biasX,
4389 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4390 final Acceleration by = new Acceleration(biasY,
4391 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4392 final Acceleration bz = new Acceleration(biasZ,
4393 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394
4395 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4396 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
4397 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4398
4399
4400 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4401 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4402 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4403 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4404 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4405 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4406 final Acceleration bx2 = new Acceleration(0.0,
4407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4408 calibrator.getBiasXAsAcceleration(bx2);
4409 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4410 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4411 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4412 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4413 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414 final Acceleration by2 = new Acceleration(0.0,
4415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4416 calibrator.getBiasYAsAcceleration(by2);
4417 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4418 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4419 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4420 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4421 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4422 final Acceleration bz2 = new Acceleration(0.0,
4423 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4424 calibrator.getBiasZAsAcceleration(bz2);
4425 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4426 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4427 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4428 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4429 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4430 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4431 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4432 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4433 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4434 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4435 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4436 final double[] bias1 = calibrator.getBias();
4437 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4438 final double[] bias2 = new double[3];
4439 calibrator.getBias(bias2);
4440 assertArrayEquals(bias1, bias2, 0.0);
4441 final Matrix b1 = calibrator.getBiasAsMatrix();
4442 assertEquals(b1, ba);
4443 final Matrix b2 = new Matrix(3, 1);
4444 calibrator.getBiasAsMatrix(b2);
4445 assertEquals(b1, b2);
4446 final Matrix ma1 = new Matrix(3, 3);
4447 ma1.setSubmatrix(0, 0,
4448 2, 2,
4449 new double[]{sx, myx, mzx,
4450 mxy, sy, mzy,
4451 mxz, myz, sz});
4452 assertEquals(calibrator.getInitialMa(), ma1);
4453 final Matrix ma2 = new Matrix(3, 3);
4454 calibrator.getInitialMa(ma2);
4455 assertEquals(ma1, ma2);
4456 assertNull(calibrator.getMeasurements());
4457 assertFalse(calibrator.isCommonAxisUsed());
4458 assertNull(calibrator.getListener());
4459 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4460 assertFalse(calibrator.isReady());
4461 assertFalse(calibrator.isRunning());
4462 assertNull(calibrator.getEstimatedMa());
4463 assertNull(calibrator.getEstimatedSx());
4464 assertNull(calibrator.getEstimatedSy());
4465 assertNull(calibrator.getEstimatedSz());
4466 assertNull(calibrator.getEstimatedMxy());
4467 assertNull(calibrator.getEstimatedMxz());
4468 assertNull(calibrator.getEstimatedMyx());
4469 assertNull(calibrator.getEstimatedMyz());
4470 assertNull(calibrator.getEstimatedMzx());
4471 assertNull(calibrator.getEstimatedMzy());
4472 assertNull(calibrator.getEstimatedCovariance());
4473 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4474 assertNull(calibrator.getGroundTruthGravityNorm());
4475 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4476 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4477 }
4478
4479 @Test
4480 public void testConstructor48() throws WrongSizeException {
4481 final Matrix ba = generateBa();
4482 final double biasX = ba.getElementAtIndex(0);
4483 final double biasY = ba.getElementAtIndex(1);
4484 final double biasZ = ba.getElementAtIndex(2);
4485
4486 final Matrix ma = generateMaCommonAxis();
4487 final double sx = ma.getElementAt(0, 0);
4488 final double sy = ma.getElementAt(1, 1);
4489 final double sz = ma.getElementAt(2, 2);
4490 final double mxy = ma.getElementAt(0, 1);
4491 final double mxz = ma.getElementAt(0, 2);
4492 final double myx = ma.getElementAt(1, 0);
4493 final double myz = ma.getElementAt(1, 2);
4494 final double mzx = ma.getElementAt(2, 0);
4495 final double mzy = ma.getElementAt(2, 1);
4496
4497 final Acceleration bx = new Acceleration(biasX,
4498 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4499 final Acceleration by = new Acceleration(biasY,
4500 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4501 final Acceleration bz = new Acceleration(biasZ,
4502 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4503
4504 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4505 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
4506 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4507
4508
4509 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4510 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4511 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4512 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4513 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4514 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4515 final Acceleration bx2 = new Acceleration(0.0,
4516 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4517 calibrator.getBiasXAsAcceleration(bx2);
4518 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4519 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4520 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4521 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4522 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4523 final Acceleration by2 = new Acceleration(0.0,
4524 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4525 calibrator.getBiasYAsAcceleration(by2);
4526 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4527 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4528 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4529 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4530 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4531 final Acceleration bz2 = new Acceleration(0.0,
4532 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4533 calibrator.getBiasZAsAcceleration(bz2);
4534 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4535 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4536 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4537 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4538 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4539 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4540 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4541 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4542 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4543 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4544 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4545 final double[] bias1 = calibrator.getBias();
4546 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4547 final double[] bias2 = new double[3];
4548 calibrator.getBias(bias2);
4549 assertArrayEquals(bias1, bias2, 0.0);
4550 final Matrix b1 = calibrator.getBiasAsMatrix();
4551 assertEquals(b1, ba);
4552 final Matrix b2 = new Matrix(3, 1);
4553 calibrator.getBiasAsMatrix(b2);
4554 assertEquals(b1, b2);
4555 final Matrix ma1 = new Matrix(3, 3);
4556 ma1.setSubmatrix(0, 0,
4557 2, 2,
4558 new double[]{sx, myx, mzx,
4559 mxy, sy, mzy,
4560 mxz, myz, sz});
4561 assertEquals(calibrator.getInitialMa(), ma1);
4562 final Matrix ma2 = new Matrix(3, 3);
4563 calibrator.getInitialMa(ma2);
4564 assertEquals(ma1, ma2);
4565 assertNull(calibrator.getMeasurements());
4566 assertFalse(calibrator.isCommonAxisUsed());
4567 assertSame(calibrator.getListener(), this);
4568 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4569 assertFalse(calibrator.isReady());
4570 assertFalse(calibrator.isRunning());
4571 assertNull(calibrator.getEstimatedMa());
4572 assertNull(calibrator.getEstimatedSx());
4573 assertNull(calibrator.getEstimatedSy());
4574 assertNull(calibrator.getEstimatedSz());
4575 assertNull(calibrator.getEstimatedMxy());
4576 assertNull(calibrator.getEstimatedMxz());
4577 assertNull(calibrator.getEstimatedMyx());
4578 assertNull(calibrator.getEstimatedMyz());
4579 assertNull(calibrator.getEstimatedMzx());
4580 assertNull(calibrator.getEstimatedMzy());
4581 assertNull(calibrator.getEstimatedCovariance());
4582 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4583 assertNull(calibrator.getGroundTruthGravityNorm());
4584 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4585 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4586 }
4587
4588 @Test
4589 public void testConstructor49() throws WrongSizeException {
4590
4591 final Collection<StandardDeviationBodyKinematics> measurements =
4592 Collections.emptyList();
4593
4594 final Matrix ba = generateBa();
4595 final double biasX = ba.getElementAtIndex(0);
4596 final double biasY = ba.getElementAtIndex(1);
4597 final double biasZ = ba.getElementAtIndex(2);
4598
4599 final Matrix ma = generateMaCommonAxis();
4600 final double sx = ma.getElementAt(0, 0);
4601 final double sy = ma.getElementAt(1, 1);
4602 final double sz = ma.getElementAt(2, 2);
4603 final double mxy = ma.getElementAt(0, 1);
4604 final double mxz = ma.getElementAt(0, 2);
4605 final double myx = ma.getElementAt(1, 0);
4606 final double myz = ma.getElementAt(1, 2);
4607 final double mzx = ma.getElementAt(2, 0);
4608 final double mzy = ma.getElementAt(2, 1);
4609
4610 final Acceleration bx = new Acceleration(biasX,
4611 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4612 final Acceleration by = new Acceleration(biasY,
4613 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4614 final Acceleration bz = new Acceleration(biasZ,
4615 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4616
4617 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4618 new KnownBiasAndGravityNormAccelerometerCalibrator(
4619 measurements, bx, by, bz, sx, sy, sz,
4620 mxy, mxz, myx, myz, mzx, mzy);
4621
4622
4623 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4624 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4625 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4626 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4627 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4628 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4629 final Acceleration bx2 = new Acceleration(0.0,
4630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4631 calibrator.getBiasXAsAcceleration(bx2);
4632 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4633 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4634 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4635 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4636 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4637 final Acceleration by2 = new Acceleration(0.0,
4638 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4639 calibrator.getBiasYAsAcceleration(by2);
4640 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4641 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4642 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4643 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4644 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4645 final Acceleration bz2 = new Acceleration(0.0,
4646 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4647 calibrator.getBiasZAsAcceleration(bz2);
4648 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4649 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4650 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4651 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4652 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4653 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4654 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4655 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4656 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4657 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4658 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4659 final double[] bias1 = calibrator.getBias();
4660 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4661 final double[] bias2 = new double[3];
4662 calibrator.getBias(bias2);
4663 assertArrayEquals(bias1, bias2, 0.0);
4664 final Matrix b1 = calibrator.getBiasAsMatrix();
4665 assertEquals(b1, ba);
4666 final Matrix b2 = new Matrix(3, 1);
4667 calibrator.getBiasAsMatrix(b2);
4668 assertEquals(b1, b2);
4669 final Matrix ma1 = new Matrix(3, 3);
4670 ma1.setSubmatrix(0, 0,
4671 2, 2,
4672 new double[]{sx, myx, mzx,
4673 mxy, sy, mzy,
4674 mxz, myz, sz});
4675 assertEquals(calibrator.getInitialMa(), ma1);
4676 final Matrix ma2 = new Matrix(3, 3);
4677 calibrator.getInitialMa(ma2);
4678 assertEquals(ma1, ma2);
4679 assertSame(calibrator.getMeasurements(), measurements);
4680 assertFalse(calibrator.isCommonAxisUsed());
4681 assertNull(calibrator.getListener());
4682 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4683 assertFalse(calibrator.isReady());
4684 assertFalse(calibrator.isRunning());
4685 assertNull(calibrator.getEstimatedMa());
4686 assertNull(calibrator.getEstimatedSx());
4687 assertNull(calibrator.getEstimatedSy());
4688 assertNull(calibrator.getEstimatedSz());
4689 assertNull(calibrator.getEstimatedMxy());
4690 assertNull(calibrator.getEstimatedMxz());
4691 assertNull(calibrator.getEstimatedMyx());
4692 assertNull(calibrator.getEstimatedMyz());
4693 assertNull(calibrator.getEstimatedMzx());
4694 assertNull(calibrator.getEstimatedMzy());
4695 assertNull(calibrator.getEstimatedCovariance());
4696 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4697 assertNull(calibrator.getGroundTruthGravityNorm());
4698 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4699 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4700 }
4701
4702 @Test
4703 public void testConstructor50() throws WrongSizeException {
4704
4705 final Collection<StandardDeviationBodyKinematics> measurements =
4706 Collections.emptyList();
4707
4708 final Matrix ba = generateBa();
4709 final double biasX = ba.getElementAtIndex(0);
4710 final double biasY = ba.getElementAtIndex(1);
4711 final double biasZ = ba.getElementAtIndex(2);
4712
4713 final Matrix ma = generateMaCommonAxis();
4714 final double sx = ma.getElementAt(0, 0);
4715 final double sy = ma.getElementAt(1, 1);
4716 final double sz = ma.getElementAt(2, 2);
4717 final double mxy = ma.getElementAt(0, 1);
4718 final double mxz = ma.getElementAt(0, 2);
4719 final double myx = ma.getElementAt(1, 0);
4720 final double myz = ma.getElementAt(1, 2);
4721 final double mzx = ma.getElementAt(2, 0);
4722 final double mzy = ma.getElementAt(2, 1);
4723
4724 final Acceleration bx = new Acceleration(biasX,
4725 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4726 final Acceleration by = new Acceleration(biasY,
4727 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4728 final Acceleration bz = new Acceleration(biasZ,
4729 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4730
4731 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4732 new KnownBiasAndGravityNormAccelerometerCalibrator(
4733 measurements, bx, by, bz, sx, sy, sz,
4734 mxy, mxz, myx, myz, mzx, mzy, this);
4735
4736
4737 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4738 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4739 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4740 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4741 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4742 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4743 final Acceleration bx2 = new Acceleration(0.0,
4744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4745 calibrator.getBiasXAsAcceleration(bx2);
4746 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4747 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4748 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4749 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4750 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4751 final Acceleration by2 = new Acceleration(0.0,
4752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4753 calibrator.getBiasYAsAcceleration(by2);
4754 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4755 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4756 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4757 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4758 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4759 final Acceleration bz2 = new Acceleration(0.0,
4760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4761 calibrator.getBiasZAsAcceleration(bz2);
4762 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4763 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4764 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4765 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4766 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4767 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4768 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4769 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4770 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4771 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4772 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4773 final double[] bias1 = calibrator.getBias();
4774 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4775 final double[] bias2 = new double[3];
4776 calibrator.getBias(bias2);
4777 assertArrayEquals(bias1, bias2, 0.0);
4778 final Matrix b1 = calibrator.getBiasAsMatrix();
4779 assertEquals(b1, ba);
4780 final Matrix b2 = new Matrix(3, 1);
4781 calibrator.getBiasAsMatrix(b2);
4782 assertEquals(b1, b2);
4783 final Matrix ma1 = new Matrix(3, 3);
4784 ma1.setSubmatrix(0, 0,
4785 2, 2,
4786 new double[]{sx, myx, mzx,
4787 mxy, sy, mzy,
4788 mxz, myz, sz});
4789 assertEquals(calibrator.getInitialMa(), ma1);
4790 final Matrix ma2 = new Matrix(3, 3);
4791 calibrator.getInitialMa(ma2);
4792 assertEquals(ma1, ma2);
4793 assertSame(calibrator.getMeasurements(), measurements);
4794 assertFalse(calibrator.isCommonAxisUsed());
4795 assertSame(calibrator.getListener(), this);
4796 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4797 assertFalse(calibrator.isReady());
4798 assertFalse(calibrator.isRunning());
4799 assertNull(calibrator.getEstimatedMa());
4800 assertNull(calibrator.getEstimatedSx());
4801 assertNull(calibrator.getEstimatedSy());
4802 assertNull(calibrator.getEstimatedSz());
4803 assertNull(calibrator.getEstimatedMxy());
4804 assertNull(calibrator.getEstimatedMxz());
4805 assertNull(calibrator.getEstimatedMyx());
4806 assertNull(calibrator.getEstimatedMyz());
4807 assertNull(calibrator.getEstimatedMzx());
4808 assertNull(calibrator.getEstimatedMzy());
4809 assertNull(calibrator.getEstimatedCovariance());
4810 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4811 assertNull(calibrator.getGroundTruthGravityNorm());
4812 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4813 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4814 }
4815
4816 @Test
4817 public void testConstructor51() throws WrongSizeException {
4818 final Matrix ba = generateBa();
4819 final double biasX = ba.getElementAtIndex(0);
4820 final double biasY = ba.getElementAtIndex(1);
4821 final double biasZ = ba.getElementAtIndex(2);
4822
4823 final Matrix ma = generateMaCommonAxis();
4824 final double sx = ma.getElementAt(0, 0);
4825 final double sy = ma.getElementAt(1, 1);
4826 final double sz = ma.getElementAt(2, 2);
4827 final double mxy = ma.getElementAt(0, 1);
4828 final double mxz = ma.getElementAt(0, 2);
4829 final double myx = ma.getElementAt(1, 0);
4830 final double myz = ma.getElementAt(1, 2);
4831 final double mzx = ma.getElementAt(2, 0);
4832 final double mzy = ma.getElementAt(2, 1);
4833
4834 final Acceleration bx = new Acceleration(biasX,
4835 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4836 final Acceleration by = new Acceleration(biasY,
4837 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4838 final Acceleration bz = new Acceleration(biasZ,
4839 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4840
4841 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4842 new KnownBiasAndGravityNormAccelerometerCalibrator(
4843 true, bx, by, bz, sx, sy, sz,
4844 mxy, mxz, myx, myz, mzx, mzy);
4845
4846
4847 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4848 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4849 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4850 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4851 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4852 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4853 final Acceleration bx2 = new Acceleration(0.0,
4854 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4855 calibrator.getBiasXAsAcceleration(bx2);
4856 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4857 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4858 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4859 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4860 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4861 final Acceleration by2 = new Acceleration(0.0,
4862 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4863 calibrator.getBiasYAsAcceleration(by2);
4864 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4865 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4866 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4867 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4868 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4869 final Acceleration bz2 = new Acceleration(0.0,
4870 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4871 calibrator.getBiasZAsAcceleration(bz2);
4872 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4873 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4874 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4875 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4876 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4877 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4878 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4879 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4880 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4881 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4882 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4883 final double[] bias1 = calibrator.getBias();
4884 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4885 final double[] bias2 = new double[3];
4886 calibrator.getBias(bias2);
4887 assertArrayEquals(bias1, bias2, 0.0);
4888 final Matrix b1 = calibrator.getBiasAsMatrix();
4889 assertEquals(b1, ba);
4890 final Matrix b2 = new Matrix(3, 1);
4891 calibrator.getBiasAsMatrix(b2);
4892 assertEquals(b1, b2);
4893 final Matrix ma1 = new Matrix(3, 3);
4894 ma1.setSubmatrix(0, 0,
4895 2, 2,
4896 new double[]{sx, myx, mzx,
4897 mxy, sy, mzy,
4898 mxz, myz, sz});
4899 assertEquals(calibrator.getInitialMa(), ma1);
4900 final Matrix ma2 = new Matrix(3, 3);
4901 calibrator.getInitialMa(ma2);
4902 assertEquals(ma1, ma2);
4903 assertNull(calibrator.getMeasurements());
4904 assertTrue(calibrator.isCommonAxisUsed());
4905 assertNull(calibrator.getListener());
4906 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4907 assertFalse(calibrator.isReady());
4908 assertFalse(calibrator.isRunning());
4909 assertNull(calibrator.getEstimatedMa());
4910 assertNull(calibrator.getEstimatedSx());
4911 assertNull(calibrator.getEstimatedSy());
4912 assertNull(calibrator.getEstimatedSz());
4913 assertNull(calibrator.getEstimatedMxy());
4914 assertNull(calibrator.getEstimatedMxz());
4915 assertNull(calibrator.getEstimatedMyx());
4916 assertNull(calibrator.getEstimatedMyz());
4917 assertNull(calibrator.getEstimatedMzx());
4918 assertNull(calibrator.getEstimatedMzy());
4919 assertNull(calibrator.getEstimatedCovariance());
4920 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4921 assertNull(calibrator.getGroundTruthGravityNorm());
4922 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4923 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4924 }
4925
4926 @Test
4927 public void testConstructor52() throws WrongSizeException {
4928 final Matrix ba = generateBa();
4929 final double biasX = ba.getElementAtIndex(0);
4930 final double biasY = ba.getElementAtIndex(1);
4931 final double biasZ = ba.getElementAtIndex(2);
4932
4933 final Matrix ma = generateMaCommonAxis();
4934 final double sx = ma.getElementAt(0, 0);
4935 final double sy = ma.getElementAt(1, 1);
4936 final double sz = ma.getElementAt(2, 2);
4937 final double mxy = ma.getElementAt(0, 1);
4938 final double mxz = ma.getElementAt(0, 2);
4939 final double myx = ma.getElementAt(1, 0);
4940 final double myz = ma.getElementAt(1, 2);
4941 final double mzx = ma.getElementAt(2, 0);
4942 final double mzy = ma.getElementAt(2, 1);
4943
4944 final Acceleration bx = new Acceleration(biasX,
4945 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4946 final Acceleration by = new Acceleration(biasY,
4947 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4948 final Acceleration bz = new Acceleration(biasZ,
4949 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4950
4951 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4952 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4953 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
4954 this);
4955
4956
4957 assertEquals(calibrator.getBiasX(), biasX, 0.0);
4958 assertEquals(calibrator.getBiasY(), biasY, 0.0);
4959 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4960 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4961 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4962 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4963 final Acceleration bx2 = new Acceleration(0.0,
4964 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4965 calibrator.getBiasXAsAcceleration(bx2);
4966 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4967 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4968 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4969 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4970 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4971 final Acceleration by2 = new Acceleration(0.0,
4972 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4973 calibrator.getBiasYAsAcceleration(by2);
4974 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4975 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4976 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4977 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4978 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4979 final Acceleration bz2 = new Acceleration(0.0,
4980 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4981 calibrator.getBiasZAsAcceleration(bz2);
4982 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4983 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4984 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4985 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4986 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4987 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4988 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4989 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4990 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4991 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4992 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4993 final double[] bias1 = calibrator.getBias();
4994 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4995 final double[] bias2 = new double[3];
4996 calibrator.getBias(bias2);
4997 assertArrayEquals(bias1, bias2, 0.0);
4998 final Matrix b1 = calibrator.getBiasAsMatrix();
4999 assertEquals(b1, ba);
5000 final Matrix b2 = new Matrix(3, 1);
5001 calibrator.getBiasAsMatrix(b2);
5002 assertEquals(b1, b2);
5003 final Matrix ma1 = new Matrix(3, 3);
5004 ma1.setSubmatrix(0, 0,
5005 2, 2,
5006 new double[]{sx, myx, mzx,
5007 mxy, sy, mzy,
5008 mxz, myz, sz});
5009 assertEquals(calibrator.getInitialMa(), ma1);
5010 final Matrix ma2 = new Matrix(3, 3);
5011 calibrator.getInitialMa(ma2);
5012 assertEquals(ma1, ma2);
5013 assertNull(calibrator.getMeasurements());
5014 assertTrue(calibrator.isCommonAxisUsed());
5015 assertSame(calibrator.getListener(), this);
5016 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5017 assertFalse(calibrator.isReady());
5018 assertFalse(calibrator.isRunning());
5019 assertNull(calibrator.getEstimatedMa());
5020 assertNull(calibrator.getEstimatedSx());
5021 assertNull(calibrator.getEstimatedSy());
5022 assertNull(calibrator.getEstimatedSz());
5023 assertNull(calibrator.getEstimatedMxy());
5024 assertNull(calibrator.getEstimatedMxz());
5025 assertNull(calibrator.getEstimatedMyx());
5026 assertNull(calibrator.getEstimatedMyz());
5027 assertNull(calibrator.getEstimatedMzx());
5028 assertNull(calibrator.getEstimatedMzy());
5029 assertNull(calibrator.getEstimatedCovariance());
5030 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5031 assertNull(calibrator.getGroundTruthGravityNorm());
5032 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5033 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5034 }
5035
5036 @Test
5037 public void testConstructor53() throws WrongSizeException {
5038 final Collection<StandardDeviationBodyKinematics> measurements =
5039 Collections.emptyList();
5040
5041 final Matrix ba = generateBa();
5042 final double biasX = ba.getElementAtIndex(0);
5043 final double biasY = ba.getElementAtIndex(1);
5044 final double biasZ = ba.getElementAtIndex(2);
5045
5046 final Matrix ma = generateMaCommonAxis();
5047 final double sx = ma.getElementAt(0, 0);
5048 final double sy = ma.getElementAt(1, 1);
5049 final double sz = ma.getElementAt(2, 2);
5050 final double mxy = ma.getElementAt(0, 1);
5051 final double mxz = ma.getElementAt(0, 2);
5052 final double myx = ma.getElementAt(1, 0);
5053 final double myz = ma.getElementAt(1, 2);
5054 final double mzx = ma.getElementAt(2, 0);
5055 final double mzy = ma.getElementAt(2, 1);
5056
5057 final Acceleration bx = new Acceleration(biasX,
5058 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5059 final Acceleration by = new Acceleration(biasY,
5060 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5061 final Acceleration bz = new Acceleration(biasZ,
5062 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5063
5064 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5065 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5066 true, bx, by, bz, sx, sy, sz,
5067 mxy, mxz, myx, myz, mzx, mzy);
5068
5069
5070 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5071 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5072 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5073 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5074 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5075 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5076 final Acceleration bx2 = new Acceleration(0.0,
5077 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5078 calibrator.getBiasXAsAcceleration(bx2);
5079 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5080 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5081 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5082 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5083 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5084 final Acceleration by2 = new Acceleration(0.0,
5085 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5086 calibrator.getBiasYAsAcceleration(by2);
5087 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5088 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5089 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5090 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5091 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5092 final Acceleration bz2 = new Acceleration(0.0,
5093 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5094 calibrator.getBiasZAsAcceleration(bz2);
5095 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5096 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5097 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5098 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5099 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5100 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5101 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5102 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5103 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5104 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5105 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5106 final double[] bias1 = calibrator.getBias();
5107 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5108 final double[] bias2 = new double[3];
5109 calibrator.getBias(bias2);
5110 assertArrayEquals(bias1, bias2, 0.0);
5111 final Matrix b1 = calibrator.getBiasAsMatrix();
5112 assertEquals(b1, ba);
5113 final Matrix b2 = new Matrix(3, 1);
5114 calibrator.getBiasAsMatrix(b2);
5115 assertEquals(b1, b2);
5116 final Matrix ma1 = new Matrix(3, 3);
5117 ma1.setSubmatrix(0, 0,
5118 2, 2,
5119 new double[]{sx, myx, mzx,
5120 mxy, sy, mzy,
5121 mxz, myz, sz});
5122 assertEquals(calibrator.getInitialMa(), ma1);
5123 final Matrix ma2 = new Matrix(3, 3);
5124 calibrator.getInitialMa(ma2);
5125 assertEquals(ma1, ma2);
5126 assertSame(calibrator.getMeasurements(), measurements);
5127 assertTrue(calibrator.isCommonAxisUsed());
5128 assertNull(calibrator.getListener());
5129 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5130 assertFalse(calibrator.isReady());
5131 assertFalse(calibrator.isRunning());
5132 assertNull(calibrator.getEstimatedMa());
5133 assertNull(calibrator.getEstimatedSx());
5134 assertNull(calibrator.getEstimatedSy());
5135 assertNull(calibrator.getEstimatedSz());
5136 assertNull(calibrator.getEstimatedMxy());
5137 assertNull(calibrator.getEstimatedMxz());
5138 assertNull(calibrator.getEstimatedMyx());
5139 assertNull(calibrator.getEstimatedMyz());
5140 assertNull(calibrator.getEstimatedMzx());
5141 assertNull(calibrator.getEstimatedMzy());
5142 assertNull(calibrator.getEstimatedCovariance());
5143 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5144 assertNull(calibrator.getGroundTruthGravityNorm());
5145 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5146 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5147 }
5148
5149 @Test
5150 public void testConstructor54() throws WrongSizeException {
5151 final Collection<StandardDeviationBodyKinematics> measurements =
5152 Collections.emptyList();
5153
5154 final Matrix ba = generateBa();
5155 final double biasX = ba.getElementAtIndex(0);
5156 final double biasY = ba.getElementAtIndex(1);
5157 final double biasZ = ba.getElementAtIndex(2);
5158
5159 final Matrix ma = generateMaCommonAxis();
5160 final double sx = ma.getElementAt(0, 0);
5161 final double sy = ma.getElementAt(1, 1);
5162 final double sz = ma.getElementAt(2, 2);
5163 final double mxy = ma.getElementAt(0, 1);
5164 final double mxz = ma.getElementAt(0, 2);
5165 final double myx = ma.getElementAt(1, 0);
5166 final double myz = ma.getElementAt(1, 2);
5167 final double mzx = ma.getElementAt(2, 0);
5168 final double mzy = ma.getElementAt(2, 1);
5169
5170 final Acceleration bx = new Acceleration(biasX,
5171 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5172 final Acceleration by = new Acceleration(biasY,
5173 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5174 final Acceleration bz = new Acceleration(biasZ,
5175 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5176
5177 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5178 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5179 true, bx, by, bz, sx, sy, sz,
5180 mxy, mxz, myx, myz, mzx, mzy, this);
5181
5182
5183 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5184 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5185 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5186 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5187 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5188 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5189 final Acceleration bx2 = new Acceleration(0.0,
5190 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5191 calibrator.getBiasXAsAcceleration(bx2);
5192 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5193 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5194 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5195 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5196 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5197 final Acceleration by2 = new Acceleration(0.0,
5198 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5199 calibrator.getBiasYAsAcceleration(by2);
5200 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5201 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5202 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5203 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5204 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5205 final Acceleration bz2 = new Acceleration(0.0,
5206 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5207 calibrator.getBiasZAsAcceleration(bz2);
5208 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5209 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5210 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5211 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5212 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5213 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5214 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5215 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5216 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5217 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5218 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5219 final double[] bias1 = calibrator.getBias();
5220 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5221 final double[] bias2 = new double[3];
5222 calibrator.getBias(bias2);
5223 assertArrayEquals(bias1, bias2, 0.0);
5224 final Matrix b1 = calibrator.getBiasAsMatrix();
5225 assertEquals(b1, ba);
5226 final Matrix b2 = new Matrix(3, 1);
5227 calibrator.getBiasAsMatrix(b2);
5228 assertEquals(b1, b2);
5229 final Matrix ma1 = new Matrix(3, 3);
5230 ma1.setSubmatrix(0, 0,
5231 2, 2,
5232 new double[]{sx, myx, mzx,
5233 mxy, sy, mzy,
5234 mxz, myz, sz});
5235 assertEquals(calibrator.getInitialMa(), ma1);
5236 final Matrix ma2 = new Matrix(3, 3);
5237 calibrator.getInitialMa(ma2);
5238 assertEquals(ma1, ma2);
5239 assertSame(calibrator.getMeasurements(), measurements);
5240 assertTrue(calibrator.isCommonAxisUsed());
5241 assertSame(calibrator.getListener(), this);
5242 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5243 assertFalse(calibrator.isReady());
5244 assertFalse(calibrator.isRunning());
5245 assertNull(calibrator.getEstimatedMa());
5246 assertNull(calibrator.getEstimatedSx());
5247 assertNull(calibrator.getEstimatedSy());
5248 assertNull(calibrator.getEstimatedSz());
5249 assertNull(calibrator.getEstimatedMxy());
5250 assertNull(calibrator.getEstimatedMxz());
5251 assertNull(calibrator.getEstimatedMyx());
5252 assertNull(calibrator.getEstimatedMyz());
5253 assertNull(calibrator.getEstimatedMzx());
5254 assertNull(calibrator.getEstimatedMzy());
5255 assertNull(calibrator.getEstimatedCovariance());
5256 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5257 assertNull(calibrator.getGroundTruthGravityNorm());
5258 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5259 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5260 }
5261
5262 @Test
5263 public void testConstructor55() throws WrongSizeException {
5264 final Matrix ba = generateBa();
5265 final double[] bias = ba.getBuffer();
5266 final double biasX = ba.getElementAtIndex(0);
5267 final double biasY = ba.getElementAtIndex(1);
5268 final double biasZ = ba.getElementAtIndex(2);
5269
5270 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5271 new KnownBiasAndGravityNormAccelerometerCalibrator(bias);
5272
5273
5274 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5275 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5276 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5277 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5278 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5279 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5280 final Acceleration bx2 = new Acceleration(0.0,
5281 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5282 calibrator.getBiasXAsAcceleration(bx2);
5283 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5284 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5285 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5286 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5287 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5288 final Acceleration by2 = new Acceleration(0.0,
5289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5290 calibrator.getBiasYAsAcceleration(by2);
5291 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5292 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5293 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5294 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5295 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5296 final Acceleration bz2 = new Acceleration(0.0,
5297 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5298 calibrator.getBiasZAsAcceleration(bz2);
5299 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5300 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5301 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5302 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5303 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5304 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5305 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5306 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5307 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5308 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5309 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5310 final double[] bias1 = calibrator.getBias();
5311 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5312 final double[] bias2 = new double[3];
5313 calibrator.getBias(bias2);
5314 assertArrayEquals(bias1, bias2, 0.0);
5315 final Matrix b1 = calibrator.getBiasAsMatrix();
5316 assertEquals(b1, ba);
5317 final Matrix b2 = new Matrix(3, 1);
5318 calibrator.getBiasAsMatrix(b2);
5319 assertEquals(b1, b2);
5320 final Matrix ma1 = calibrator.getInitialMa();
5321 assertEquals(ma1, new Matrix(3, 3));
5322 final Matrix ma2 = new Matrix(3, 3);
5323 calibrator.getInitialMa(ma2);
5324 assertEquals(ma1, ma2);
5325 assertNull(calibrator.getMeasurements());
5326 assertFalse(calibrator.isCommonAxisUsed());
5327 assertNull(calibrator.getListener());
5328 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5329 assertFalse(calibrator.isReady());
5330 assertFalse(calibrator.isRunning());
5331 assertNull(calibrator.getEstimatedMa());
5332 assertNull(calibrator.getEstimatedSx());
5333 assertNull(calibrator.getEstimatedSy());
5334 assertNull(calibrator.getEstimatedSz());
5335 assertNull(calibrator.getEstimatedMxy());
5336 assertNull(calibrator.getEstimatedMxz());
5337 assertNull(calibrator.getEstimatedMyx());
5338 assertNull(calibrator.getEstimatedMyz());
5339 assertNull(calibrator.getEstimatedMzx());
5340 assertNull(calibrator.getEstimatedMzy());
5341 assertNull(calibrator.getEstimatedCovariance());
5342 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5343 assertNull(calibrator.getGroundTruthGravityNorm());
5344 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5345 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5346
5347
5348 calibrator = null;
5349 try {
5350 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5351 new double[1]);
5352 fail("IllegalArgumentException expected but not thrown");
5353 } catch (final IllegalArgumentException ignore) {
5354 }
5355 assertNull(calibrator);
5356 }
5357
5358 @Test
5359 public void testConstructor56() throws WrongSizeException {
5360 final Matrix ba = generateBa();
5361 final double[] bias = ba.getBuffer();
5362 final double biasX = ba.getElementAtIndex(0);
5363 final double biasY = ba.getElementAtIndex(1);
5364 final double biasZ = ba.getElementAtIndex(2);
5365
5366 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5367 new KnownBiasAndGravityNormAccelerometerCalibrator(bias,
5368 this);
5369
5370
5371 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5372 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5373 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5374 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5375 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5376 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5377 final Acceleration bx2 = new Acceleration(0.0,
5378 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5379 calibrator.getBiasXAsAcceleration(bx2);
5380 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5381 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5383 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5384 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5385 final Acceleration by2 = new Acceleration(0.0,
5386 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5387 calibrator.getBiasYAsAcceleration(by2);
5388 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5389 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5390 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5391 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5392 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5393 final Acceleration bz2 = new Acceleration(0.0,
5394 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5395 calibrator.getBiasZAsAcceleration(bz2);
5396 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5397 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5398 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5399 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5400 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5401 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5402 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5403 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5404 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5405 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5406 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5407 final double[] bias1 = calibrator.getBias();
5408 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5409 final double[] bias2 = new double[3];
5410 calibrator.getBias(bias2);
5411 assertArrayEquals(bias1, bias2, 0.0);
5412 final Matrix b1 = calibrator.getBiasAsMatrix();
5413 assertEquals(b1, ba);
5414 final Matrix b2 = new Matrix(3, 1);
5415 calibrator.getBiasAsMatrix(b2);
5416 assertEquals(b1, b2);
5417 final Matrix ma1 = calibrator.getInitialMa();
5418 assertEquals(ma1, new Matrix(3, 3));
5419 final Matrix ma2 = new Matrix(3, 3);
5420 calibrator.getInitialMa(ma2);
5421 assertEquals(ma1, ma2);
5422 assertNull(calibrator.getMeasurements());
5423 assertFalse(calibrator.isCommonAxisUsed());
5424 assertSame(calibrator.getListener(), this);
5425 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5426 assertFalse(calibrator.isReady());
5427 assertFalse(calibrator.isRunning());
5428 assertNull(calibrator.getEstimatedMa());
5429 assertNull(calibrator.getEstimatedSx());
5430 assertNull(calibrator.getEstimatedSy());
5431 assertNull(calibrator.getEstimatedSz());
5432 assertNull(calibrator.getEstimatedMxy());
5433 assertNull(calibrator.getEstimatedMxz());
5434 assertNull(calibrator.getEstimatedMyx());
5435 assertNull(calibrator.getEstimatedMyz());
5436 assertNull(calibrator.getEstimatedMzx());
5437 assertNull(calibrator.getEstimatedMzy());
5438 assertNull(calibrator.getEstimatedCovariance());
5439 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5440 assertNull(calibrator.getGroundTruthGravityNorm());
5441 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5442 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5443
5444
5445 calibrator = null;
5446 try {
5447 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5448 new double[1], this);
5449 fail("IllegalArgumentException expected but not thrown");
5450 } catch (final IllegalArgumentException ignore) {
5451 }
5452 assertNull(calibrator);
5453 }
5454
5455 @Test
5456 public void testConstructor57() throws WrongSizeException {
5457 final Collection<StandardDeviationBodyKinematics> measurements =
5458 Collections.emptyList();
5459
5460 final Matrix ba = generateBa();
5461 final double[] bias = ba.getBuffer();
5462 final double biasX = ba.getElementAtIndex(0);
5463 final double biasY = ba.getElementAtIndex(1);
5464 final double biasZ = ba.getElementAtIndex(2);
5465
5466 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5467 new KnownBiasAndGravityNormAccelerometerCalibrator(
5468 measurements, bias);
5469
5470
5471 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5472 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5473 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5474 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5475 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5476 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5477 final Acceleration bx2 = new Acceleration(0.0,
5478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5479 calibrator.getBiasXAsAcceleration(bx2);
5480 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5481 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5482 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5483 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5484 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5485 final Acceleration by2 = new Acceleration(0.0,
5486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5487 calibrator.getBiasYAsAcceleration(by2);
5488 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5489 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5491 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5492 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5493 final Acceleration bz2 = new Acceleration(0.0,
5494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5495 calibrator.getBiasZAsAcceleration(bz2);
5496 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5497 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5498 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5499 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5500 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5501 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5502 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5503 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5504 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5505 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5506 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5507 final double[] bias1 = calibrator.getBias();
5508 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5509 final double[] bias2 = new double[3];
5510 calibrator.getBias(bias2);
5511 assertArrayEquals(bias1, bias2, 0.0);
5512 final Matrix b1 = calibrator.getBiasAsMatrix();
5513 assertEquals(b1, ba);
5514 final Matrix b2 = new Matrix(3, 1);
5515 calibrator.getBiasAsMatrix(b2);
5516 assertEquals(b1, b2);
5517 final Matrix ma1 = calibrator.getInitialMa();
5518 assertEquals(ma1, new Matrix(3, 3));
5519 final Matrix ma2 = new Matrix(3, 3);
5520 calibrator.getInitialMa(ma2);
5521 assertEquals(ma1, ma2);
5522 assertSame(calibrator.getMeasurements(), measurements);
5523 assertFalse(calibrator.isCommonAxisUsed());
5524 assertNull(calibrator.getListener());
5525 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5526 assertFalse(calibrator.isReady());
5527 assertFalse(calibrator.isRunning());
5528 assertNull(calibrator.getEstimatedMa());
5529 assertNull(calibrator.getEstimatedSx());
5530 assertNull(calibrator.getEstimatedSy());
5531 assertNull(calibrator.getEstimatedSz());
5532 assertNull(calibrator.getEstimatedMxy());
5533 assertNull(calibrator.getEstimatedMxz());
5534 assertNull(calibrator.getEstimatedMyx());
5535 assertNull(calibrator.getEstimatedMyz());
5536 assertNull(calibrator.getEstimatedMzx());
5537 assertNull(calibrator.getEstimatedMzy());
5538 assertNull(calibrator.getEstimatedCovariance());
5539 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5540 assertNull(calibrator.getGroundTruthGravityNorm());
5541 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5542 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5543
5544
5545 calibrator = null;
5546 try {
5547 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5548 measurements, new double[1]);
5549 fail("IllegalArgumentException expected but not thrown");
5550 } catch (final IllegalArgumentException ignore) {
5551 }
5552 assertNull(calibrator);
5553 }
5554
5555 @Test
5556 public void testConstructor58() throws WrongSizeException {
5557 final Collection<StandardDeviationBodyKinematics> measurements =
5558 Collections.emptyList();
5559
5560 final Matrix ba = generateBa();
5561 final double[] bias = ba.getBuffer();
5562 final double biasX = ba.getElementAtIndex(0);
5563 final double biasY = ba.getElementAtIndex(1);
5564 final double biasZ = ba.getElementAtIndex(2);
5565
5566 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5567 new KnownBiasAndGravityNormAccelerometerCalibrator(
5568 measurements, bias, this);
5569
5570
5571 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5572 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5573 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5574 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5575 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5576 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5577 final Acceleration bx2 = new Acceleration(0.0,
5578 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5579 calibrator.getBiasXAsAcceleration(bx2);
5580 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5581 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5582 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5583 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5584 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5585 final Acceleration by2 = new Acceleration(0.0,
5586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5587 calibrator.getBiasYAsAcceleration(by2);
5588 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5589 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5590 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5591 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5592 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5593 final Acceleration bz2 = new Acceleration(0.0,
5594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5595 calibrator.getBiasZAsAcceleration(bz2);
5596 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5597 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5598 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5599 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5600 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5601 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5602 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5603 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5604 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5605 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5606 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5607 final double[] bias1 = calibrator.getBias();
5608 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5609 final double[] bias2 = new double[3];
5610 calibrator.getBias(bias2);
5611 assertArrayEquals(bias1, bias2, 0.0);
5612 final Matrix b1 = calibrator.getBiasAsMatrix();
5613 assertEquals(b1, ba);
5614 final Matrix b2 = new Matrix(3, 1);
5615 calibrator.getBiasAsMatrix(b2);
5616 assertEquals(b1, b2);
5617 final Matrix ma1 = calibrator.getInitialMa();
5618 assertEquals(ma1, new Matrix(3, 3));
5619 final Matrix ma2 = new Matrix(3, 3);
5620 calibrator.getInitialMa(ma2);
5621 assertEquals(ma1, ma2);
5622 assertSame(calibrator.getMeasurements(), measurements);
5623 assertFalse(calibrator.isCommonAxisUsed());
5624 assertSame(calibrator.getListener(), this);
5625 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5626 assertFalse(calibrator.isReady());
5627 assertFalse(calibrator.isRunning());
5628 assertNull(calibrator.getEstimatedMa());
5629 assertNull(calibrator.getEstimatedSx());
5630 assertNull(calibrator.getEstimatedSy());
5631 assertNull(calibrator.getEstimatedSz());
5632 assertNull(calibrator.getEstimatedMxy());
5633 assertNull(calibrator.getEstimatedMxz());
5634 assertNull(calibrator.getEstimatedMyx());
5635 assertNull(calibrator.getEstimatedMyz());
5636 assertNull(calibrator.getEstimatedMzx());
5637 assertNull(calibrator.getEstimatedMzy());
5638 assertNull(calibrator.getEstimatedCovariance());
5639 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5640 assertNull(calibrator.getGroundTruthGravityNorm());
5641 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5642 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5643
5644
5645 calibrator = null;
5646 try {
5647 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5648 measurements, new double[1], this);
5649 fail("IllegalArgumentException expected but not thrown");
5650 } catch (final IllegalArgumentException ignore) {
5651 }
5652 assertNull(calibrator);
5653 }
5654
5655 @Test
5656 public void testConstructor59() throws WrongSizeException {
5657 final Matrix ba = generateBa();
5658 final double[] bias = ba.getBuffer();
5659 final double biasX = ba.getElementAtIndex(0);
5660 final double biasY = ba.getElementAtIndex(1);
5661 final double biasZ = ba.getElementAtIndex(2);
5662
5663 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5664 new KnownBiasAndGravityNormAccelerometerCalibrator(
5665 true, bias);
5666
5667
5668 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5669 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5670 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5671 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5672 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5673 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5674 final Acceleration bx2 = new Acceleration(0.0,
5675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5676 calibrator.getBiasXAsAcceleration(bx2);
5677 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5678 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5679 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5680 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5681 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5682 final Acceleration by2 = new Acceleration(0.0,
5683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5684 calibrator.getBiasYAsAcceleration(by2);
5685 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5686 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5687 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5688 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5689 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5690 final Acceleration bz2 = new Acceleration(0.0,
5691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5692 calibrator.getBiasZAsAcceleration(bz2);
5693 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5694 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5695 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5696 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5697 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5698 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5699 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5700 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5701 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5702 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5703 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5704 final double[] bias1 = calibrator.getBias();
5705 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5706 final double[] bias2 = new double[3];
5707 calibrator.getBias(bias2);
5708 assertArrayEquals(bias1, bias2, 0.0);
5709 final Matrix b1 = calibrator.getBiasAsMatrix();
5710 assertEquals(b1, ba);
5711 final Matrix b2 = new Matrix(3, 1);
5712 calibrator.getBiasAsMatrix(b2);
5713 assertEquals(b1, b2);
5714 final Matrix ma1 = calibrator.getInitialMa();
5715 assertEquals(ma1, new Matrix(3, 3));
5716 final Matrix ma2 = new Matrix(3, 3);
5717 calibrator.getInitialMa(ma2);
5718 assertEquals(ma1, ma2);
5719 assertNull(calibrator.getMeasurements());
5720 assertTrue(calibrator.isCommonAxisUsed());
5721 assertNull(calibrator.getListener());
5722 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5723 assertFalse(calibrator.isReady());
5724 assertFalse(calibrator.isRunning());
5725 assertNull(calibrator.getEstimatedMa());
5726 assertNull(calibrator.getEstimatedSx());
5727 assertNull(calibrator.getEstimatedSy());
5728 assertNull(calibrator.getEstimatedSz());
5729 assertNull(calibrator.getEstimatedMxy());
5730 assertNull(calibrator.getEstimatedMxz());
5731 assertNull(calibrator.getEstimatedMyx());
5732 assertNull(calibrator.getEstimatedMyz());
5733 assertNull(calibrator.getEstimatedMzx());
5734 assertNull(calibrator.getEstimatedMzy());
5735 assertNull(calibrator.getEstimatedCovariance());
5736 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5737 assertNull(calibrator.getGroundTruthGravityNorm());
5738 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5739 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5740
5741
5742 calibrator = null;
5743 try {
5744 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5745 true, new double[1]);
5746 fail("IllegalArgumentException expected but not thrown");
5747 } catch (final IllegalArgumentException ignore) {
5748 }
5749 assertNull(calibrator);
5750 }
5751
5752 @Test
5753 public void testConstructor60() throws WrongSizeException {
5754 final Matrix ba = generateBa();
5755 final double[] bias = ba.getBuffer();
5756 final double biasX = ba.getElementAtIndex(0);
5757 final double biasY = ba.getElementAtIndex(1);
5758 final double biasZ = ba.getElementAtIndex(2);
5759
5760 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5761 new KnownBiasAndGravityNormAccelerometerCalibrator(
5762 true, bias, this);
5763
5764
5765 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5766 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5767 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5768 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5769 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5770 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5771 final Acceleration bx2 = new Acceleration(0.0,
5772 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5773 calibrator.getBiasXAsAcceleration(bx2);
5774 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5775 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5776 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5777 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5778 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5779 final Acceleration by2 = new Acceleration(0.0,
5780 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5781 calibrator.getBiasYAsAcceleration(by2);
5782 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5783 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5784 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5785 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5786 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5787 final Acceleration bz2 = new Acceleration(0.0,
5788 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5789 calibrator.getBiasZAsAcceleration(bz2);
5790 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5791 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5792 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5793 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5794 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5795 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5796 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5797 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5798 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5799 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5800 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5801 final double[] bias1 = calibrator.getBias();
5802 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5803 final double[] bias2 = new double[3];
5804 calibrator.getBias(bias2);
5805 assertArrayEquals(bias1, bias2, 0.0);
5806 final Matrix b1 = calibrator.getBiasAsMatrix();
5807 assertEquals(b1, ba);
5808 final Matrix b2 = new Matrix(3, 1);
5809 calibrator.getBiasAsMatrix(b2);
5810 assertEquals(b1, b2);
5811 final Matrix ma1 = calibrator.getInitialMa();
5812 assertEquals(ma1, new Matrix(3, 3));
5813 final Matrix ma2 = new Matrix(3, 3);
5814 calibrator.getInitialMa(ma2);
5815 assertEquals(ma1, ma2);
5816 assertNull(calibrator.getMeasurements());
5817 assertTrue(calibrator.isCommonAxisUsed());
5818 assertSame(calibrator.getListener(), this);
5819 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5820 assertFalse(calibrator.isReady());
5821 assertFalse(calibrator.isRunning());
5822 assertNull(calibrator.getEstimatedMa());
5823 assertNull(calibrator.getEstimatedSx());
5824 assertNull(calibrator.getEstimatedSy());
5825 assertNull(calibrator.getEstimatedSz());
5826 assertNull(calibrator.getEstimatedMxy());
5827 assertNull(calibrator.getEstimatedMxz());
5828 assertNull(calibrator.getEstimatedMyx());
5829 assertNull(calibrator.getEstimatedMyz());
5830 assertNull(calibrator.getEstimatedMzx());
5831 assertNull(calibrator.getEstimatedMzy());
5832 assertNull(calibrator.getEstimatedCovariance());
5833 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5834 assertNull(calibrator.getGroundTruthGravityNorm());
5835 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5836 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5837
5838
5839 calibrator = null;
5840 try {
5841 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5842 true, new double[1], this);
5843 fail("IllegalArgumentException expected but not thrown");
5844 } catch (final IllegalArgumentException ignore) {
5845 }
5846 assertNull(calibrator);
5847 }
5848
5849 @Test
5850 public void testConstructor61() throws WrongSizeException {
5851 final Collection<StandardDeviationBodyKinematics> measurements =
5852 Collections.emptyList();
5853
5854 final Matrix ba = generateBa();
5855 final double[] bias = ba.getBuffer();
5856 final double biasX = ba.getElementAtIndex(0);
5857 final double biasY = ba.getElementAtIndex(1);
5858 final double biasZ = ba.getElementAtIndex(2);
5859
5860 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5861 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5862 true, bias);
5863
5864
5865 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5866 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5867 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5868 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5869 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5870 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5871 final Acceleration bx2 = new Acceleration(0.0,
5872 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5873 calibrator.getBiasXAsAcceleration(bx2);
5874 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5875 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5876 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5877 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5878 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5879 final Acceleration by2 = new Acceleration(0.0,
5880 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5881 calibrator.getBiasYAsAcceleration(by2);
5882 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5883 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5884 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5885 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5886 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5887 final Acceleration bz2 = new Acceleration(0.0,
5888 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5889 calibrator.getBiasZAsAcceleration(bz2);
5890 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5891 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5892 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5893 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5894 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5895 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5896 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5897 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5898 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5899 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5900 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5901 final double[] bias1 = calibrator.getBias();
5902 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5903 final double[] bias2 = new double[3];
5904 calibrator.getBias(bias2);
5905 assertArrayEquals(bias1, bias2, 0.0);
5906 final Matrix b1 = calibrator.getBiasAsMatrix();
5907 assertEquals(b1, ba);
5908 final Matrix b2 = new Matrix(3, 1);
5909 calibrator.getBiasAsMatrix(b2);
5910 assertEquals(b1, b2);
5911 final Matrix ma1 = calibrator.getInitialMa();
5912 assertEquals(ma1, new Matrix(3, 3));
5913 final Matrix ma2 = new Matrix(3, 3);
5914 calibrator.getInitialMa(ma2);
5915 assertEquals(ma1, ma2);
5916 assertSame(calibrator.getMeasurements(), measurements);
5917 assertTrue(calibrator.isCommonAxisUsed());
5918 assertNull(calibrator.getListener());
5919 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5920 assertFalse(calibrator.isReady());
5921 assertFalse(calibrator.isRunning());
5922 assertNull(calibrator.getEstimatedMa());
5923 assertNull(calibrator.getEstimatedSx());
5924 assertNull(calibrator.getEstimatedSy());
5925 assertNull(calibrator.getEstimatedSz());
5926 assertNull(calibrator.getEstimatedMxy());
5927 assertNull(calibrator.getEstimatedMxz());
5928 assertNull(calibrator.getEstimatedMyx());
5929 assertNull(calibrator.getEstimatedMyz());
5930 assertNull(calibrator.getEstimatedMzx());
5931 assertNull(calibrator.getEstimatedMzy());
5932 assertNull(calibrator.getEstimatedCovariance());
5933 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5934 assertNull(calibrator.getGroundTruthGravityNorm());
5935 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5936 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5937
5938
5939 calibrator = null;
5940 try {
5941 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5942 measurements, true, new double[1]);
5943 fail("IllegalArgumentException expected but not thrown");
5944 } catch (final IllegalArgumentException ignore) {
5945 }
5946 assertNull(calibrator);
5947 }
5948
5949 @Test
5950 public void testConstructor62() throws WrongSizeException {
5951 final Collection<StandardDeviationBodyKinematics> measurements =
5952 Collections.emptyList();
5953
5954 final Matrix ba = generateBa();
5955 final double[] bias = ba.getBuffer();
5956 final double biasX = ba.getElementAtIndex(0);
5957 final double biasY = ba.getElementAtIndex(1);
5958 final double biasZ = ba.getElementAtIndex(2);
5959
5960 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5961 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5962 true, bias, this);
5963
5964
5965 assertEquals(calibrator.getBiasX(), biasX, 0.0);
5966 assertEquals(calibrator.getBiasY(), biasY, 0.0);
5967 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5968 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5969 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5970 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5971 final Acceleration bx2 = new Acceleration(0.0,
5972 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5973 calibrator.getBiasXAsAcceleration(bx2);
5974 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5975 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5976 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5977 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5978 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5979 final Acceleration by2 = new Acceleration(0.0,
5980 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5981 calibrator.getBiasYAsAcceleration(by2);
5982 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5983 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5984 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5985 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5986 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5987 final Acceleration bz2 = new Acceleration(0.0,
5988 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5989 calibrator.getBiasZAsAcceleration(bz2);
5990 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5991 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5992 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5993 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5994 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5995 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5996 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5997 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5998 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5999 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6000 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6001 final double[] bias1 = calibrator.getBias();
6002 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6003 final double[] bias2 = new double[3];
6004 calibrator.getBias(bias2);
6005 assertArrayEquals(bias1, bias2, 0.0);
6006 final Matrix b1 = calibrator.getBiasAsMatrix();
6007 assertEquals(b1, ba);
6008 final Matrix b2 = new Matrix(3, 1);
6009 calibrator.getBiasAsMatrix(b2);
6010 assertEquals(b1, b2);
6011 final Matrix ma1 = calibrator.getInitialMa();
6012 assertEquals(ma1, new Matrix(3, 3));
6013 final Matrix ma2 = new Matrix(3, 3);
6014 calibrator.getInitialMa(ma2);
6015 assertEquals(ma1, ma2);
6016 assertSame(calibrator.getMeasurements(), measurements);
6017 assertTrue(calibrator.isCommonAxisUsed());
6018 assertSame(calibrator.getListener(), this);
6019 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6020 assertFalse(calibrator.isReady());
6021 assertFalse(calibrator.isRunning());
6022 assertNull(calibrator.getEstimatedMa());
6023 assertNull(calibrator.getEstimatedSx());
6024 assertNull(calibrator.getEstimatedSy());
6025 assertNull(calibrator.getEstimatedSz());
6026 assertNull(calibrator.getEstimatedMxy());
6027 assertNull(calibrator.getEstimatedMxz());
6028 assertNull(calibrator.getEstimatedMyx());
6029 assertNull(calibrator.getEstimatedMyz());
6030 assertNull(calibrator.getEstimatedMzx());
6031 assertNull(calibrator.getEstimatedMzy());
6032 assertNull(calibrator.getEstimatedCovariance());
6033 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6034 assertNull(calibrator.getGroundTruthGravityNorm());
6035 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6036 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6037
6038
6039 calibrator = null;
6040 try {
6041 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6042 measurements, true, new double[1],
6043 this);
6044 fail("IllegalArgumentException expected but not thrown");
6045 } catch (final IllegalArgumentException ignore) {
6046 }
6047 assertNull(calibrator);
6048 }
6049
6050 @Test
6051 public void testConstructor63() throws WrongSizeException {
6052 final Matrix ba = generateBa();
6053 final double biasX = ba.getElementAtIndex(0);
6054 final double biasY = ba.getElementAtIndex(1);
6055 final double biasZ = ba.getElementAtIndex(2);
6056
6057 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6058 new KnownBiasAndGravityNormAccelerometerCalibrator(ba);
6059
6060
6061 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6062 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6063 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6064 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6065 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6066 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6067 final Acceleration bx2 = new Acceleration(0.0,
6068 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6069 calibrator.getBiasXAsAcceleration(bx2);
6070 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6071 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6072 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6073 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6074 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6075 final Acceleration by2 = new Acceleration(0.0,
6076 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6077 calibrator.getBiasYAsAcceleration(by2);
6078 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6079 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6080 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6081 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6082 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6083 final Acceleration bz2 = new Acceleration(0.0,
6084 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6085 calibrator.getBiasZAsAcceleration(bz2);
6086 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6087 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6088 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6089 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6090 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6091 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6092 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6093 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6094 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6095 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6096 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6097 final double[] bias1 = calibrator.getBias();
6098 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6099 final double[] bias2 = new double[3];
6100 calibrator.getBias(bias2);
6101 assertArrayEquals(bias1, bias2, 0.0);
6102 final Matrix b1 = calibrator.getBiasAsMatrix();
6103 assertEquals(b1, ba);
6104 final Matrix b2 = new Matrix(3, 1);
6105 calibrator.getBiasAsMatrix(b2);
6106 assertEquals(b1, b2);
6107 final Matrix ma1 = calibrator.getInitialMa();
6108 assertEquals(ma1, new Matrix(3, 3));
6109 final Matrix ma2 = new Matrix(3, 3);
6110 calibrator.getInitialMa(ma2);
6111 assertEquals(ma1, ma2);
6112 assertNull(calibrator.getMeasurements());
6113 assertFalse(calibrator.isCommonAxisUsed());
6114 assertNull(calibrator.getListener());
6115 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6116 assertFalse(calibrator.isReady());
6117 assertFalse(calibrator.isRunning());
6118 assertNull(calibrator.getEstimatedMa());
6119 assertNull(calibrator.getEstimatedSx());
6120 assertNull(calibrator.getEstimatedSy());
6121 assertNull(calibrator.getEstimatedSz());
6122 assertNull(calibrator.getEstimatedMxy());
6123 assertNull(calibrator.getEstimatedMxz());
6124 assertNull(calibrator.getEstimatedMyx());
6125 assertNull(calibrator.getEstimatedMyz());
6126 assertNull(calibrator.getEstimatedMzx());
6127 assertNull(calibrator.getEstimatedMzy());
6128 assertNull(calibrator.getEstimatedCovariance());
6129 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6130 assertNull(calibrator.getGroundTruthGravityNorm());
6131 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6132 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6133
6134
6135 calibrator = null;
6136 try {
6137 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6138 new Matrix(1, 1));
6139 fail("IllegalArgumentException expected but not thrown");
6140 } catch (final IllegalArgumentException ignore) {
6141 }
6142 try {
6143 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6144 new Matrix(1, 3));
6145 fail("IllegalArgumentException expected but not thrown");
6146 } catch (final IllegalArgumentException ignore) {
6147 }
6148 assertNull(calibrator);
6149 }
6150
6151 @Test
6152 public void testConstructor64() throws WrongSizeException {
6153 final Matrix ba = generateBa();
6154 final double biasX = ba.getElementAtIndex(0);
6155 final double biasY = ba.getElementAtIndex(1);
6156 final double biasZ = ba.getElementAtIndex(2);
6157
6158 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6159 new KnownBiasAndGravityNormAccelerometerCalibrator(
6160 ba, this);
6161
6162
6163 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6164 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6165 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6166 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6167 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6168 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6169 final Acceleration bx2 = new Acceleration(0.0,
6170 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6171 calibrator.getBiasXAsAcceleration(bx2);
6172 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6173 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6174 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6175 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6176 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6177 final Acceleration by2 = new Acceleration(0.0,
6178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6179 calibrator.getBiasYAsAcceleration(by2);
6180 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6181 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6182 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6183 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6184 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6185 final Acceleration bz2 = new Acceleration(0.0,
6186 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6187 calibrator.getBiasZAsAcceleration(bz2);
6188 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6189 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6190 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6191 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6192 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6193 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6194 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6195 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6196 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6197 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6198 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6199 final double[] bias1 = calibrator.getBias();
6200 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6201 final double[] bias2 = new double[3];
6202 calibrator.getBias(bias2);
6203 assertArrayEquals(bias1, bias2, 0.0);
6204 final Matrix b1 = calibrator.getBiasAsMatrix();
6205 assertEquals(b1, ba);
6206 final Matrix b2 = new Matrix(3, 1);
6207 calibrator.getBiasAsMatrix(b2);
6208 assertEquals(b1, b2);
6209 final Matrix ma1 = calibrator.getInitialMa();
6210 assertEquals(ma1, new Matrix(3, 3));
6211 final Matrix ma2 = new Matrix(3, 3);
6212 calibrator.getInitialMa(ma2);
6213 assertEquals(ma1, ma2);
6214 assertNull(calibrator.getMeasurements());
6215 assertFalse(calibrator.isCommonAxisUsed());
6216 assertSame(calibrator.getListener(), this);
6217 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6218 assertFalse(calibrator.isReady());
6219 assertFalse(calibrator.isRunning());
6220 assertNull(calibrator.getEstimatedMa());
6221 assertNull(calibrator.getEstimatedSx());
6222 assertNull(calibrator.getEstimatedSy());
6223 assertNull(calibrator.getEstimatedSz());
6224 assertNull(calibrator.getEstimatedMxy());
6225 assertNull(calibrator.getEstimatedMxz());
6226 assertNull(calibrator.getEstimatedMyx());
6227 assertNull(calibrator.getEstimatedMyz());
6228 assertNull(calibrator.getEstimatedMzx());
6229 assertNull(calibrator.getEstimatedMzy());
6230 assertNull(calibrator.getEstimatedCovariance());
6231 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6232 assertNull(calibrator.getGroundTruthGravityNorm());
6233 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6234 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6235
6236
6237 calibrator = null;
6238 try {
6239 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6240 new Matrix(1, 1));
6241 fail("IllegalArgumentException expected but not thrown");
6242 } catch (final IllegalArgumentException ignore) {
6243 }
6244 try {
6245 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6246 new Matrix(1, 3));
6247 fail("IllegalArgumentException expected but not thrown");
6248 } catch (final IllegalArgumentException ignore) {
6249 }
6250 assertNull(calibrator);
6251 }
6252
6253 @Test
6254 public void testConstructor65() throws WrongSizeException {
6255 final Collection<StandardDeviationBodyKinematics> measurements =
6256 Collections.emptyList();
6257
6258 final Matrix ba = generateBa();
6259 final double biasX = ba.getElementAtIndex(0);
6260 final double biasY = ba.getElementAtIndex(1);
6261 final double biasZ = ba.getElementAtIndex(2);
6262
6263 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6264 new KnownBiasAndGravityNormAccelerometerCalibrator(
6265 measurements, ba);
6266
6267
6268 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6269 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6270 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6271 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6272 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6273 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6274 final Acceleration bx2 = new Acceleration(0.0,
6275 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6276 calibrator.getBiasXAsAcceleration(bx2);
6277 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6278 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6279 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6280 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6281 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6282 final Acceleration by2 = new Acceleration(0.0,
6283 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6284 calibrator.getBiasYAsAcceleration(by2);
6285 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6286 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6287 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6288 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6289 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6290 final Acceleration bz2 = new Acceleration(0.0,
6291 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6292 calibrator.getBiasZAsAcceleration(bz2);
6293 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6294 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6295 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6296 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6297 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6298 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6299 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6300 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6301 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6302 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6303 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6304 final double[] bias1 = calibrator.getBias();
6305 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6306 final double[] bias2 = new double[3];
6307 calibrator.getBias(bias2);
6308 assertArrayEquals(bias1, bias2, 0.0);
6309 final Matrix b1 = calibrator.getBiasAsMatrix();
6310 assertEquals(b1, ba);
6311 final Matrix b2 = new Matrix(3, 1);
6312 calibrator.getBiasAsMatrix(b2);
6313 assertEquals(b1, b2);
6314 final Matrix ma1 = calibrator.getInitialMa();
6315 assertEquals(ma1, new Matrix(3, 3));
6316 final Matrix ma2 = new Matrix(3, 3);
6317 calibrator.getInitialMa(ma2);
6318 assertEquals(ma1, ma2);
6319 assertSame(calibrator.getMeasurements(), measurements);
6320 assertFalse(calibrator.isCommonAxisUsed());
6321 assertNull(calibrator.getListener());
6322 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6323 assertFalse(calibrator.isReady());
6324 assertFalse(calibrator.isRunning());
6325 assertNull(calibrator.getEstimatedMa());
6326 assertNull(calibrator.getEstimatedSx());
6327 assertNull(calibrator.getEstimatedSy());
6328 assertNull(calibrator.getEstimatedSz());
6329 assertNull(calibrator.getEstimatedMxy());
6330 assertNull(calibrator.getEstimatedMxz());
6331 assertNull(calibrator.getEstimatedMyx());
6332 assertNull(calibrator.getEstimatedMyz());
6333 assertNull(calibrator.getEstimatedMzx());
6334 assertNull(calibrator.getEstimatedMzy());
6335 assertNull(calibrator.getEstimatedCovariance());
6336 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6337 assertNull(calibrator.getGroundTruthGravityNorm());
6338 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6339 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6340
6341
6342 calibrator = null;
6343 try {
6344 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6345 new Matrix(1, 1));
6346 fail("IllegalArgumentException expected but not thrown");
6347 } catch (final IllegalArgumentException ignore) {
6348 }
6349 try {
6350 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6351 new Matrix(1, 3));
6352 fail("IllegalArgumentException expected but not thrown");
6353 } catch (final IllegalArgumentException ignore) {
6354 }
6355 assertNull(calibrator);
6356 }
6357
6358 @Test
6359 public void testConstructor66() throws WrongSizeException {
6360 final Collection<StandardDeviationBodyKinematics> measurements =
6361 Collections.emptyList();
6362
6363 final Matrix ba = generateBa();
6364 final double biasX = ba.getElementAtIndex(0);
6365 final double biasY = ba.getElementAtIndex(1);
6366 final double biasZ = ba.getElementAtIndex(2);
6367
6368 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6369 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba,
6370 this);
6371
6372
6373 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6374 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6375 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6376 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6377 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6378 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6379 final Acceleration bx2 = new Acceleration(0.0,
6380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6381 calibrator.getBiasXAsAcceleration(bx2);
6382 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6383 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6384 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6385 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6386 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6387 final Acceleration by2 = new Acceleration(0.0,
6388 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6389 calibrator.getBiasYAsAcceleration(by2);
6390 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6391 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6392 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6393 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6394 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6395 final Acceleration bz2 = new Acceleration(0.0,
6396 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6397 calibrator.getBiasZAsAcceleration(bz2);
6398 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6399 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6400 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6401 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6402 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6403 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6404 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6405 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6406 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6407 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6408 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6409 final double[] bias1 = calibrator.getBias();
6410 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6411 final double[] bias2 = new double[3];
6412 calibrator.getBias(bias2);
6413 assertArrayEquals(bias1, bias2, 0.0);
6414 final Matrix b1 = calibrator.getBiasAsMatrix();
6415 assertEquals(b1, ba);
6416 final Matrix b2 = new Matrix(3, 1);
6417 calibrator.getBiasAsMatrix(b2);
6418 assertEquals(b1, b2);
6419 final Matrix ma1 = calibrator.getInitialMa();
6420 assertEquals(ma1, new Matrix(3, 3));
6421 final Matrix ma2 = new Matrix(3, 3);
6422 calibrator.getInitialMa(ma2);
6423 assertEquals(ma1, ma2);
6424 assertSame(calibrator.getMeasurements(), measurements);
6425 assertFalse(calibrator.isCommonAxisUsed());
6426 assertSame(calibrator.getListener(), this);
6427 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6428 assertFalse(calibrator.isReady());
6429 assertFalse(calibrator.isRunning());
6430 assertNull(calibrator.getEstimatedMa());
6431 assertNull(calibrator.getEstimatedSx());
6432 assertNull(calibrator.getEstimatedSy());
6433 assertNull(calibrator.getEstimatedSz());
6434 assertNull(calibrator.getEstimatedMxy());
6435 assertNull(calibrator.getEstimatedMxz());
6436 assertNull(calibrator.getEstimatedMyx());
6437 assertNull(calibrator.getEstimatedMyz());
6438 assertNull(calibrator.getEstimatedMzx());
6439 assertNull(calibrator.getEstimatedMzy());
6440 assertNull(calibrator.getEstimatedCovariance());
6441 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6442 assertNull(calibrator.getGroundTruthGravityNorm());
6443 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6444 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6445
6446
6447 calibrator = null;
6448 try {
6449 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6450 measurements, new Matrix(1, 1),
6451 this);
6452 fail("IllegalArgumentException expected but not thrown");
6453 } catch (final IllegalArgumentException ignore) {
6454 }
6455 try {
6456 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6457 measurements, new Matrix(1, 3),
6458 this);
6459 fail("IllegalArgumentException expected but not thrown");
6460 } catch (final IllegalArgumentException ignore) {
6461 }
6462 assertNull(calibrator);
6463 }
6464
6465 @Test
6466 public void testConstructor67() throws WrongSizeException {
6467 final Matrix ba = generateBa();
6468 final double biasX = ba.getElementAtIndex(0);
6469 final double biasY = ba.getElementAtIndex(1);
6470 final double biasZ = ba.getElementAtIndex(2);
6471
6472 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6473 new KnownBiasAndGravityNormAccelerometerCalibrator(
6474 true, ba);
6475
6476
6477 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6478 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6479 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6480 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6481 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6482 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6483 final Acceleration bx2 = new Acceleration(0.0,
6484 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6485 calibrator.getBiasXAsAcceleration(bx2);
6486 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6487 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6488 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6489 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6490 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6491 final Acceleration by2 = new Acceleration(0.0,
6492 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6493 calibrator.getBiasYAsAcceleration(by2);
6494 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6495 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6496 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6497 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6498 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6499 final Acceleration bz2 = new Acceleration(0.0,
6500 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6501 calibrator.getBiasZAsAcceleration(bz2);
6502 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6503 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6504 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6505 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6506 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6507 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6508 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6509 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6510 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6511 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6512 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6513 final double[] bias1 = calibrator.getBias();
6514 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6515 final double[] bias2 = new double[3];
6516 calibrator.getBias(bias2);
6517 assertArrayEquals(bias1, bias2, 0.0);
6518 final Matrix b1 = calibrator.getBiasAsMatrix();
6519 assertEquals(b1, ba);
6520 final Matrix b2 = new Matrix(3, 1);
6521 calibrator.getBiasAsMatrix(b2);
6522 assertEquals(b1, b2);
6523 final Matrix ma1 = calibrator.getInitialMa();
6524 assertEquals(ma1, new Matrix(3, 3));
6525 final Matrix ma2 = new Matrix(3, 3);
6526 calibrator.getInitialMa(ma2);
6527 assertEquals(ma1, ma2);
6528 assertNull(calibrator.getMeasurements());
6529 assertTrue(calibrator.isCommonAxisUsed());
6530 assertNull(calibrator.getListener());
6531 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6532 assertFalse(calibrator.isReady());
6533 assertFalse(calibrator.isRunning());
6534 assertNull(calibrator.getEstimatedMa());
6535 assertNull(calibrator.getEstimatedSx());
6536 assertNull(calibrator.getEstimatedSy());
6537 assertNull(calibrator.getEstimatedSz());
6538 assertNull(calibrator.getEstimatedMxy());
6539 assertNull(calibrator.getEstimatedMxz());
6540 assertNull(calibrator.getEstimatedMyx());
6541 assertNull(calibrator.getEstimatedMyz());
6542 assertNull(calibrator.getEstimatedMzx());
6543 assertNull(calibrator.getEstimatedMzy());
6544 assertNull(calibrator.getEstimatedCovariance());
6545 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6546 assertNull(calibrator.getGroundTruthGravityNorm());
6547 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6548 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6549
6550
6551 calibrator = null;
6552 try {
6553 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6554 true, new Matrix(1, 1));
6555 fail("IllegalArgumentException expected but not thrown");
6556 } catch (final IllegalArgumentException ignore) {
6557 }
6558 try {
6559 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6560 true, new Matrix(1, 3));
6561 fail("IllegalArgumentException expected but not thrown");
6562 } catch (final IllegalArgumentException ignore) {
6563 }
6564 assertNull(calibrator);
6565 }
6566
6567 @Test
6568 public void testConstructor68() throws WrongSizeException {
6569 final Matrix ba = generateBa();
6570 final double biasX = ba.getElementAtIndex(0);
6571 final double biasY = ba.getElementAtIndex(1);
6572 final double biasZ = ba.getElementAtIndex(2);
6573
6574 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6575 new KnownBiasAndGravityNormAccelerometerCalibrator(
6576 true, ba, this);
6577
6578
6579 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6580 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6581 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6582 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6583 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6584 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6585 final Acceleration bx2 = new Acceleration(0.0,
6586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6587 calibrator.getBiasXAsAcceleration(bx2);
6588 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6589 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6590 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6591 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6592 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6593 final Acceleration by2 = new Acceleration(0.0,
6594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6595 calibrator.getBiasYAsAcceleration(by2);
6596 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6597 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6598 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6599 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6600 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6601 final Acceleration bz2 = new Acceleration(0.0,
6602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6603 calibrator.getBiasZAsAcceleration(bz2);
6604 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6605 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6606 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6607 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6608 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6609 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6610 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6611 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6612 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6613 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6614 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6615 final double[] bias1 = calibrator.getBias();
6616 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6617 final double[] bias2 = new double[3];
6618 calibrator.getBias(bias2);
6619 assertArrayEquals(bias1, bias2, 0.0);
6620 final Matrix b1 = calibrator.getBiasAsMatrix();
6621 assertEquals(b1, ba);
6622 final Matrix b2 = new Matrix(3, 1);
6623 calibrator.getBiasAsMatrix(b2);
6624 assertEquals(b1, b2);
6625 final Matrix ma1 = calibrator.getInitialMa();
6626 assertEquals(ma1, new Matrix(3, 3));
6627 final Matrix ma2 = new Matrix(3, 3);
6628 calibrator.getInitialMa(ma2);
6629 assertEquals(ma1, ma2);
6630 assertNull(calibrator.getMeasurements());
6631 assertTrue(calibrator.isCommonAxisUsed());
6632 assertSame(calibrator.getListener(), this);
6633 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6634 assertFalse(calibrator.isReady());
6635 assertFalse(calibrator.isRunning());
6636 assertNull(calibrator.getEstimatedMa());
6637 assertNull(calibrator.getEstimatedSx());
6638 assertNull(calibrator.getEstimatedSy());
6639 assertNull(calibrator.getEstimatedSz());
6640 assertNull(calibrator.getEstimatedMxy());
6641 assertNull(calibrator.getEstimatedMxz());
6642 assertNull(calibrator.getEstimatedMyx());
6643 assertNull(calibrator.getEstimatedMyz());
6644 assertNull(calibrator.getEstimatedMzx());
6645 assertNull(calibrator.getEstimatedMzy());
6646 assertNull(calibrator.getEstimatedCovariance());
6647 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6648 assertNull(calibrator.getGroundTruthGravityNorm());
6649 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6650 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6651
6652
6653 calibrator = null;
6654 try {
6655 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6656 true, new Matrix(1, 1),
6657 this);
6658 fail("IllegalArgumentException expected but not thrown");
6659 } catch (final IllegalArgumentException ignore) {
6660 }
6661 try {
6662 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6663 true, new Matrix(1, 3),
6664 this);
6665 fail("IllegalArgumentException expected but not thrown");
6666 } catch (final IllegalArgumentException ignore) {
6667 }
6668 assertNull(calibrator);
6669 }
6670
6671 @Test
6672 public void testConstructor69() throws WrongSizeException {
6673 final Collection<StandardDeviationBodyKinematics> measurements =
6674 Collections.emptyList();
6675
6676 final Matrix ba = generateBa();
6677 final double biasX = ba.getElementAtIndex(0);
6678 final double biasY = ba.getElementAtIndex(1);
6679 final double biasZ = ba.getElementAtIndex(2);
6680
6681 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6682 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6683 true, ba);
6684
6685
6686 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6687 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6688 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6689 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6690 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6691 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6692 final Acceleration bx2 = new Acceleration(0.0,
6693 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6694 calibrator.getBiasXAsAcceleration(bx2);
6695 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6696 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6697 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6698 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6699 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6700 final Acceleration by2 = new Acceleration(0.0,
6701 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6702 calibrator.getBiasYAsAcceleration(by2);
6703 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6704 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6705 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6706 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6707 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6708 final Acceleration bz2 = new Acceleration(0.0,
6709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6710 calibrator.getBiasZAsAcceleration(bz2);
6711 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6712 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6713 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6714 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6715 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6716 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6717 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6718 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6719 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6720 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6721 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6722 final double[] bias1 = calibrator.getBias();
6723 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6724 final double[] bias2 = new double[3];
6725 calibrator.getBias(bias2);
6726 assertArrayEquals(bias1, bias2, 0.0);
6727 final Matrix b1 = calibrator.getBiasAsMatrix();
6728 assertEquals(b1, ba);
6729 final Matrix b2 = new Matrix(3, 1);
6730 calibrator.getBiasAsMatrix(b2);
6731 assertEquals(b1, b2);
6732 final Matrix ma1 = calibrator.getInitialMa();
6733 assertEquals(ma1, new Matrix(3, 3));
6734 final Matrix ma2 = new Matrix(3, 3);
6735 calibrator.getInitialMa(ma2);
6736 assertEquals(ma1, ma2);
6737 assertSame(calibrator.getMeasurements(), measurements);
6738 assertTrue(calibrator.isCommonAxisUsed());
6739 assertNull(calibrator.getListener());
6740 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6741 assertFalse(calibrator.isReady());
6742 assertFalse(calibrator.isRunning());
6743 assertNull(calibrator.getEstimatedMa());
6744 assertNull(calibrator.getEstimatedSx());
6745 assertNull(calibrator.getEstimatedSy());
6746 assertNull(calibrator.getEstimatedSz());
6747 assertNull(calibrator.getEstimatedMxy());
6748 assertNull(calibrator.getEstimatedMxz());
6749 assertNull(calibrator.getEstimatedMyx());
6750 assertNull(calibrator.getEstimatedMyz());
6751 assertNull(calibrator.getEstimatedMzx());
6752 assertNull(calibrator.getEstimatedMzy());
6753 assertNull(calibrator.getEstimatedCovariance());
6754 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6755 assertNull(calibrator.getGroundTruthGravityNorm());
6756 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6757 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6758
6759
6760 calibrator = null;
6761 try {
6762 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6763 measurements, true,
6764 new Matrix(1, 1));
6765 fail("IllegalArgumentException expected but not thrown");
6766 } catch (final IllegalArgumentException ignore) {
6767 }
6768 try {
6769 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6770 measurements, true,
6771 new Matrix(1, 3));
6772 fail("IllegalArgumentException expected but not thrown");
6773 } catch (final IllegalArgumentException ignore) {
6774 }
6775 assertNull(calibrator);
6776 }
6777
6778 @Test
6779 public void testConstructor70() throws WrongSizeException {
6780 final Collection<StandardDeviationBodyKinematics> measurements =
6781 Collections.emptyList();
6782
6783 final Matrix ba = generateBa();
6784 final double biasX = ba.getElementAtIndex(0);
6785 final double biasY = ba.getElementAtIndex(1);
6786 final double biasZ = ba.getElementAtIndex(2);
6787
6788 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6789 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6790 true, ba, this);
6791
6792
6793 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6794 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6795 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6796 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6797 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6798 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6799 final Acceleration bx2 = new Acceleration(0.0,
6800 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6801 calibrator.getBiasXAsAcceleration(bx2);
6802 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6803 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6804 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6805 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6806 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6807 final Acceleration by2 = new Acceleration(0.0,
6808 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6809 calibrator.getBiasYAsAcceleration(by2);
6810 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6811 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6812 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6813 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6814 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6815 final Acceleration bz2 = new Acceleration(0.0,
6816 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6817 calibrator.getBiasZAsAcceleration(bz2);
6818 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6819 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6820 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6821 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6822 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6823 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6824 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6825 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6826 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6827 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6828 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6829 final double[] bias1 = calibrator.getBias();
6830 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6831 final double[] bias2 = new double[3];
6832 calibrator.getBias(bias2);
6833 assertArrayEquals(bias1, bias2, 0.0);
6834 final Matrix b1 = calibrator.getBiasAsMatrix();
6835 assertEquals(b1, ba);
6836 final Matrix b2 = new Matrix(3, 1);
6837 calibrator.getBiasAsMatrix(b2);
6838 assertEquals(b1, b2);
6839 final Matrix ma1 = calibrator.getInitialMa();
6840 assertEquals(ma1, new Matrix(3, 3));
6841 final Matrix ma2 = new Matrix(3, 3);
6842 calibrator.getInitialMa(ma2);
6843 assertEquals(ma1, ma2);
6844 assertSame(calibrator.getMeasurements(), measurements);
6845 assertTrue(calibrator.isCommonAxisUsed());
6846 assertSame(calibrator.getListener(), this);
6847 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6848 assertFalse(calibrator.isReady());
6849 assertFalse(calibrator.isRunning());
6850 assertNull(calibrator.getEstimatedMa());
6851 assertNull(calibrator.getEstimatedSx());
6852 assertNull(calibrator.getEstimatedSy());
6853 assertNull(calibrator.getEstimatedSz());
6854 assertNull(calibrator.getEstimatedMxy());
6855 assertNull(calibrator.getEstimatedMxz());
6856 assertNull(calibrator.getEstimatedMyx());
6857 assertNull(calibrator.getEstimatedMyz());
6858 assertNull(calibrator.getEstimatedMzx());
6859 assertNull(calibrator.getEstimatedMzy());
6860 assertNull(calibrator.getEstimatedCovariance());
6861 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6862 assertNull(calibrator.getGroundTruthGravityNorm());
6863 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6864 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6865
6866
6867 calibrator = null;
6868 try {
6869 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6870 measurements, true,
6871 new Matrix(1, 1), this);
6872 fail("IllegalArgumentException expected but not thrown");
6873 } catch (final IllegalArgumentException ignore) {
6874 }
6875 try {
6876 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6877 measurements, true,
6878 new Matrix(1, 3), this);
6879 fail("IllegalArgumentException expected but not thrown");
6880 } catch (final IllegalArgumentException ignore) {
6881 }
6882 assertNull(calibrator);
6883 }
6884
6885 @Test
6886 public void testConstructor71() throws WrongSizeException {
6887 final Matrix ba = generateBa();
6888 final double biasX = ba.getElementAtIndex(0);
6889 final double biasY = ba.getElementAtIndex(1);
6890 final double biasZ = ba.getElementAtIndex(2);
6891
6892 final Matrix ma = generateMaCommonAxis();
6893 final double sx = ma.getElementAt(0, 0);
6894 final double sy = ma.getElementAt(1, 1);
6895 final double sz = ma.getElementAt(2, 2);
6896 final double mxy = ma.getElementAt(0, 1);
6897 final double mxz = ma.getElementAt(0, 2);
6898 final double myx = ma.getElementAt(1, 0);
6899 final double myz = ma.getElementAt(1, 2);
6900 final double mzx = ma.getElementAt(2, 0);
6901 final double mzy = ma.getElementAt(2, 1);
6902
6903 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6904 new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma);
6905
6906
6907 assertEquals(calibrator.getBiasX(), biasX, 0.0);
6908 assertEquals(calibrator.getBiasY(), biasY, 0.0);
6909 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6910 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6911 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6912 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6913 final Acceleration bx2 = new Acceleration(0.0,
6914 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6915 calibrator.getBiasXAsAcceleration(bx2);
6916 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6917 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6918 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6919 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6920 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6921 final Acceleration by2 = new Acceleration(0.0,
6922 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6923 calibrator.getBiasYAsAcceleration(by2);
6924 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6925 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6926 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6927 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6928 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6929 final Acceleration bz2 = new Acceleration(0.0,
6930 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6931 calibrator.getBiasZAsAcceleration(bz2);
6932 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6933 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6934 assertEquals(calibrator.getInitialSx(), sx, 0.0);
6935 assertEquals(calibrator.getInitialSy(), sy, 0.0);
6936 assertEquals(calibrator.getInitialSz(), sz, 0.0);
6937 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6938 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6939 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6940 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6941 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6942 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6943 final double[] bias1 = calibrator.getBias();
6944 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6945 final double[] bias2 = new double[3];
6946 calibrator.getBias(bias2);
6947 assertArrayEquals(bias1, bias2, 0.0);
6948 final Matrix b1 = calibrator.getBiasAsMatrix();
6949 assertEquals(b1, ba);
6950 final Matrix b2 = new Matrix(3, 1);
6951 calibrator.getBiasAsMatrix(b2);
6952 assertEquals(b1, b2);
6953 final Matrix ma1 = new Matrix(3, 3);
6954 ma1.setSubmatrix(0, 0,
6955 2, 2,
6956 new double[]{sx, myx, mzx,
6957 mxy, sy, mzy,
6958 mxz, myz, sz});
6959 assertEquals(calibrator.getInitialMa(), ma1);
6960 final Matrix ma2 = new Matrix(3, 3);
6961 calibrator.getInitialMa(ma2);
6962 assertEquals(ma1, ma2);
6963 assertNull(calibrator.getMeasurements());
6964 assertFalse(calibrator.isCommonAxisUsed());
6965 assertNull(calibrator.getListener());
6966 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6967 assertFalse(calibrator.isReady());
6968 assertFalse(calibrator.isRunning());
6969 assertNull(calibrator.getEstimatedMa());
6970 assertNull(calibrator.getEstimatedSx());
6971 assertNull(calibrator.getEstimatedSy());
6972 assertNull(calibrator.getEstimatedSz());
6973 assertNull(calibrator.getEstimatedMxy());
6974 assertNull(calibrator.getEstimatedMxz());
6975 assertNull(calibrator.getEstimatedMyx());
6976 assertNull(calibrator.getEstimatedMyz());
6977 assertNull(calibrator.getEstimatedMzx());
6978 assertNull(calibrator.getEstimatedMzy());
6979 assertNull(calibrator.getEstimatedCovariance());
6980 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6981 assertNull(calibrator.getGroundTruthGravityNorm());
6982 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6983 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6984
6985
6986 calibrator = null;
6987 try {
6988 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6989 new Matrix(1, 1), ma);
6990 fail("IllegalArgumentException expected but not thrown");
6991 } catch (final IllegalArgumentException ignore) {
6992 }
6993 try {
6994 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6995 new Matrix(1, 3), ma);
6996 fail("IllegalArgumentException expected but not thrown");
6997 } catch (final IllegalArgumentException ignore) {
6998 }
6999 try {
7000 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7001 new Matrix(1, 3));
7002 fail("IllegalArgumentException expected but not thrown");
7003 } catch (final IllegalArgumentException ignore) {
7004 }
7005 try {
7006 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7007 new Matrix(3, 1));
7008 fail("IllegalArgumentException expected but not thrown");
7009 } catch (final IllegalArgumentException ignore) {
7010 }
7011 assertNull(calibrator);
7012 }
7013
7014 @Test
7015 public void testConstructor72() throws WrongSizeException {
7016 final Matrix ba = generateBa();
7017 final double biasX = ba.getElementAtIndex(0);
7018 final double biasY = ba.getElementAtIndex(1);
7019 final double biasZ = ba.getElementAtIndex(2);
7020
7021 final Matrix ma = generateMaCommonAxis();
7022 final double sx = ma.getElementAt(0, 0);
7023 final double sy = ma.getElementAt(1, 1);
7024 final double sz = ma.getElementAt(2, 2);
7025 final double mxy = ma.getElementAt(0, 1);
7026 final double mxz = ma.getElementAt(0, 2);
7027 final double myx = ma.getElementAt(1, 0);
7028 final double myz = ma.getElementAt(1, 2);
7029 final double mzx = ma.getElementAt(2, 0);
7030 final double mzy = ma.getElementAt(2, 1);
7031
7032 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7033 new KnownBiasAndGravityNormAccelerometerCalibrator(
7034 ba, ma, this);
7035
7036
7037 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7038 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7039 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7040 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7041 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7042 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7043 final Acceleration bx2 = new Acceleration(0.0,
7044 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7045 calibrator.getBiasXAsAcceleration(bx2);
7046 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7047 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7048 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7049 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7050 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7051 final Acceleration by2 = new Acceleration(0.0,
7052 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7053 calibrator.getBiasYAsAcceleration(by2);
7054 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7055 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7056 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7057 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7058 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7059 final Acceleration bz2 = new Acceleration(0.0,
7060 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7061 calibrator.getBiasZAsAcceleration(bz2);
7062 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7063 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7064 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7065 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7066 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7067 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7068 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7069 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7070 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7071 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7072 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7073 final double[] bias1 = calibrator.getBias();
7074 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7075 final double[] bias2 = new double[3];
7076 calibrator.getBias(bias2);
7077 assertArrayEquals(bias1, bias2, 0.0);
7078 final Matrix b1 = calibrator.getBiasAsMatrix();
7079 assertEquals(b1, ba);
7080 final Matrix b2 = new Matrix(3, 1);
7081 calibrator.getBiasAsMatrix(b2);
7082 assertEquals(b1, b2);
7083 final Matrix ma1 = new Matrix(3, 3);
7084 ma1.setSubmatrix(0, 0,
7085 2, 2,
7086 new double[]{sx, myx, mzx,
7087 mxy, sy, mzy,
7088 mxz, myz, sz});
7089 assertEquals(calibrator.getInitialMa(), ma1);
7090 final Matrix ma2 = new Matrix(3, 3);
7091 calibrator.getInitialMa(ma2);
7092 assertEquals(ma1, ma2);
7093 assertNull(calibrator.getMeasurements());
7094 assertFalse(calibrator.isCommonAxisUsed());
7095 assertSame(calibrator.getListener(), this);
7096 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7097 assertFalse(calibrator.isReady());
7098 assertFalse(calibrator.isRunning());
7099 assertNull(calibrator.getEstimatedMa());
7100 assertNull(calibrator.getEstimatedSx());
7101 assertNull(calibrator.getEstimatedSy());
7102 assertNull(calibrator.getEstimatedSz());
7103 assertNull(calibrator.getEstimatedMxy());
7104 assertNull(calibrator.getEstimatedMxz());
7105 assertNull(calibrator.getEstimatedMyx());
7106 assertNull(calibrator.getEstimatedMyz());
7107 assertNull(calibrator.getEstimatedMzx());
7108 assertNull(calibrator.getEstimatedMzy());
7109 assertNull(calibrator.getEstimatedCovariance());
7110 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7111 assertNull(calibrator.getGroundTruthGravityNorm());
7112 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7113 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7114
7115
7116 calibrator = null;
7117 try {
7118 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7119 new Matrix(1, 1), ma, this);
7120 fail("IllegalArgumentException expected but not thrown");
7121 } catch (final IllegalArgumentException ignore) {
7122 }
7123 try {
7124 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7125 new Matrix(1, 3), ma, this);
7126 fail("IllegalArgumentException expected but not thrown");
7127 } catch (final IllegalArgumentException ignore) {
7128 }
7129 try {
7130 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7131 new Matrix(1, 3), this);
7132 fail("IllegalArgumentException expected but not thrown");
7133 } catch (final IllegalArgumentException ignore) {
7134 }
7135 try {
7136 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7137 new Matrix(3, 1), this);
7138 fail("IllegalArgumentException expected but not thrown");
7139 } catch (final IllegalArgumentException ignore) {
7140 }
7141 assertNull(calibrator);
7142 }
7143
7144 @Test
7145 public void testConstructor73() throws WrongSizeException {
7146 final Collection<StandardDeviationBodyKinematics> measurements =
7147 Collections.emptyList();
7148
7149 final Matrix ba = generateBa();
7150 final double biasX = ba.getElementAtIndex(0);
7151 final double biasY = ba.getElementAtIndex(1);
7152 final double biasZ = ba.getElementAtIndex(2);
7153
7154 final Matrix ma = generateMaCommonAxis();
7155 final double sx = ma.getElementAt(0, 0);
7156 final double sy = ma.getElementAt(1, 1);
7157 final double sz = ma.getElementAt(2, 2);
7158 final double mxy = ma.getElementAt(0, 1);
7159 final double mxz = ma.getElementAt(0, 2);
7160 final double myx = ma.getElementAt(1, 0);
7161 final double myz = ma.getElementAt(1, 2);
7162 final double mzx = ma.getElementAt(2, 0);
7163 final double mzy = ma.getElementAt(2, 1);
7164
7165 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7166 new KnownBiasAndGravityNormAccelerometerCalibrator(
7167 measurements, ba, ma);
7168
7169
7170 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7171 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7172 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7173 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7174 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7175 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7176 final Acceleration bx2 = new Acceleration(0.0,
7177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7178 calibrator.getBiasXAsAcceleration(bx2);
7179 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7180 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7181 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7182 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7183 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7184 final Acceleration by2 = new Acceleration(0.0,
7185 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7186 calibrator.getBiasYAsAcceleration(by2);
7187 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7188 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7189 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7190 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7191 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7192 final Acceleration bz2 = new Acceleration(0.0,
7193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7194 calibrator.getBiasZAsAcceleration(bz2);
7195 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7196 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7197 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7198 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7199 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7200 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7201 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7202 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7203 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7204 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7205 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7206 final double[] bias1 = calibrator.getBias();
7207 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7208 final double[] bias2 = new double[3];
7209 calibrator.getBias(bias2);
7210 assertArrayEquals(bias1, bias2, 0.0);
7211 final Matrix b1 = calibrator.getBiasAsMatrix();
7212 assertEquals(b1, ba);
7213 final Matrix b2 = new Matrix(3, 1);
7214 calibrator.getBiasAsMatrix(b2);
7215 assertEquals(b1, b2);
7216 final Matrix ma1 = new Matrix(3, 3);
7217 ma1.setSubmatrix(0, 0,
7218 2, 2,
7219 new double[]{sx, myx, mzx,
7220 mxy, sy, mzy,
7221 mxz, myz, sz});
7222 assertEquals(calibrator.getInitialMa(), ma1);
7223 final Matrix ma2 = new Matrix(3, 3);
7224 calibrator.getInitialMa(ma2);
7225 assertEquals(ma1, ma2);
7226 assertSame(calibrator.getMeasurements(), measurements);
7227 assertFalse(calibrator.isCommonAxisUsed());
7228 assertNull(calibrator.getListener());
7229 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7230 assertFalse(calibrator.isReady());
7231 assertFalse(calibrator.isRunning());
7232 assertNull(calibrator.getEstimatedMa());
7233 assertNull(calibrator.getEstimatedSx());
7234 assertNull(calibrator.getEstimatedSy());
7235 assertNull(calibrator.getEstimatedSz());
7236 assertNull(calibrator.getEstimatedMxy());
7237 assertNull(calibrator.getEstimatedMxz());
7238 assertNull(calibrator.getEstimatedMyx());
7239 assertNull(calibrator.getEstimatedMyz());
7240 assertNull(calibrator.getEstimatedMzx());
7241 assertNull(calibrator.getEstimatedMzy());
7242 assertNull(calibrator.getEstimatedCovariance());
7243 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7244 assertNull(calibrator.getGroundTruthGravityNorm());
7245 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7246 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7247
7248
7249 calibrator = null;
7250 try {
7251 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7252 measurements, new Matrix(1, 1), ma);
7253 fail("IllegalArgumentException expected but not thrown");
7254 } catch (final IllegalArgumentException ignore) {
7255 }
7256 try {
7257 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7258 measurements, new Matrix(1, 3), ma);
7259 fail("IllegalArgumentException expected but not thrown");
7260 } catch (final IllegalArgumentException ignore) {
7261 }
7262 try {
7263 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7264 measurements, ba, new Matrix(1, 3));
7265 fail("IllegalArgumentException expected but not thrown");
7266 } catch (final IllegalArgumentException ignore) {
7267 }
7268 try {
7269 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7270 measurements, ba, new Matrix(3, 1));
7271 fail("IllegalArgumentException expected but not thrown");
7272 } catch (final IllegalArgumentException ignore) {
7273 }
7274 assertNull(calibrator);
7275 }
7276
7277 @Test
7278 public void testConstructor74() throws WrongSizeException {
7279 final Collection<StandardDeviationBodyKinematics> measurements =
7280 Collections.emptyList();
7281
7282 final Matrix ba = generateBa();
7283 final double biasX = ba.getElementAtIndex(0);
7284 final double biasY = ba.getElementAtIndex(1);
7285 final double biasZ = ba.getElementAtIndex(2);
7286
7287 final Matrix ma = generateMaCommonAxis();
7288 final double sx = ma.getElementAt(0, 0);
7289 final double sy = ma.getElementAt(1, 1);
7290 final double sz = ma.getElementAt(2, 2);
7291 final double mxy = ma.getElementAt(0, 1);
7292 final double mxz = ma.getElementAt(0, 2);
7293 final double myx = ma.getElementAt(1, 0);
7294 final double myz = ma.getElementAt(1, 2);
7295 final double mzx = ma.getElementAt(2, 0);
7296 final double mzy = ma.getElementAt(2, 1);
7297
7298 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7299 new KnownBiasAndGravityNormAccelerometerCalibrator(
7300 measurements, ba, ma, this);
7301
7302
7303 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7304 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7305 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7306 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7307 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7308 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7309 final Acceleration bx2 = new Acceleration(0.0,
7310 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7311 calibrator.getBiasXAsAcceleration(bx2);
7312 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7313 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7314 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7315 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7316 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7317 final Acceleration by2 = new Acceleration(0.0,
7318 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7319 calibrator.getBiasYAsAcceleration(by2);
7320 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7321 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7322 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7323 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7324 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7325 final Acceleration bz2 = new Acceleration(0.0,
7326 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7327 calibrator.getBiasZAsAcceleration(bz2);
7328 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7329 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7330 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7331 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7332 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7333 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7334 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7335 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7336 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7337 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7338 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7339 final double[] bias1 = calibrator.getBias();
7340 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7341 final double[] bias2 = new double[3];
7342 calibrator.getBias(bias2);
7343 assertArrayEquals(bias1, bias2, 0.0);
7344 final Matrix b1 = calibrator.getBiasAsMatrix();
7345 assertEquals(b1, ba);
7346 final Matrix b2 = new Matrix(3, 1);
7347 calibrator.getBiasAsMatrix(b2);
7348 assertEquals(b1, b2);
7349 final Matrix ma1 = new Matrix(3, 3);
7350 ma1.setSubmatrix(0, 0,
7351 2, 2,
7352 new double[]{sx, myx, mzx,
7353 mxy, sy, mzy,
7354 mxz, myz, sz});
7355 assertEquals(calibrator.getInitialMa(), ma1);
7356 final Matrix ma2 = new Matrix(3, 3);
7357 calibrator.getInitialMa(ma2);
7358 assertEquals(ma1, ma2);
7359 assertSame(calibrator.getMeasurements(), measurements);
7360 assertFalse(calibrator.isCommonAxisUsed());
7361 assertSame(calibrator.getListener(), this);
7362 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7363 assertFalse(calibrator.isReady());
7364 assertFalse(calibrator.isRunning());
7365 assertNull(calibrator.getEstimatedMa());
7366 assertNull(calibrator.getEstimatedSx());
7367 assertNull(calibrator.getEstimatedSy());
7368 assertNull(calibrator.getEstimatedSz());
7369 assertNull(calibrator.getEstimatedMxy());
7370 assertNull(calibrator.getEstimatedMxz());
7371 assertNull(calibrator.getEstimatedMyx());
7372 assertNull(calibrator.getEstimatedMyz());
7373 assertNull(calibrator.getEstimatedMzx());
7374 assertNull(calibrator.getEstimatedMzy());
7375 assertNull(calibrator.getEstimatedCovariance());
7376 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7377 assertNull(calibrator.getGroundTruthGravityNorm());
7378 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7379 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7380
7381
7382 calibrator = null;
7383 try {
7384 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7385 measurements, new Matrix(1, 1), ma,
7386 this);
7387 fail("IllegalArgumentException expected but not thrown");
7388 } catch (final IllegalArgumentException ignore) {
7389 }
7390 try {
7391 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7392 measurements, new Matrix(1, 3), ma,
7393 this);
7394 fail("IllegalArgumentException expected but not thrown");
7395 } catch (final IllegalArgumentException ignore) {
7396 }
7397 try {
7398 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7399 measurements, ba, new Matrix(1, 3),
7400 this);
7401 fail("IllegalArgumentException expected but not thrown");
7402 } catch (final IllegalArgumentException ignore) {
7403 }
7404 try {
7405 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7406 measurements, ba, new Matrix(3, 1),
7407 this);
7408 fail("IllegalArgumentException expected but not thrown");
7409 } catch (final IllegalArgumentException ignore) {
7410 }
7411 assertNull(calibrator);
7412 }
7413
7414 @Test
7415 public void testConstructor75() throws WrongSizeException {
7416 final Matrix ba = generateBa();
7417 final double biasX = ba.getElementAtIndex(0);
7418 final double biasY = ba.getElementAtIndex(1);
7419 final double biasZ = ba.getElementAtIndex(2);
7420
7421 final Matrix ma = generateMaCommonAxis();
7422 final double sx = ma.getElementAt(0, 0);
7423 final double sy = ma.getElementAt(1, 1);
7424 final double sz = ma.getElementAt(2, 2);
7425 final double mxy = ma.getElementAt(0, 1);
7426 final double mxz = ma.getElementAt(0, 2);
7427 final double myx = ma.getElementAt(1, 0);
7428 final double myz = ma.getElementAt(1, 2);
7429 final double mzx = ma.getElementAt(2, 0);
7430 final double mzy = ma.getElementAt(2, 1);
7431
7432 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7433 new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma);
7434
7435
7436 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7437 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7438 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7439 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7440 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7441 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7442 final Acceleration bx2 = new Acceleration(0.0,
7443 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7444 calibrator.getBiasXAsAcceleration(bx2);
7445 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7446 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7447 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7448 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7449 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7450 final Acceleration by2 = new Acceleration(0.0,
7451 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7452 calibrator.getBiasYAsAcceleration(by2);
7453 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7454 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7455 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7456 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7457 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7458 final Acceleration bz2 = new Acceleration(0.0,
7459 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7460 calibrator.getBiasZAsAcceleration(bz2);
7461 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7462 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7463 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7464 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7465 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7466 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7467 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7468 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7469 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7470 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7471 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7472 final double[] bias1 = calibrator.getBias();
7473 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7474 final double[] bias2 = new double[3];
7475 calibrator.getBias(bias2);
7476 assertArrayEquals(bias1, bias2, 0.0);
7477 final Matrix b1 = calibrator.getBiasAsMatrix();
7478 assertEquals(b1, ba);
7479 final Matrix b2 = new Matrix(3, 1);
7480 calibrator.getBiasAsMatrix(b2);
7481 assertEquals(b1, b2);
7482 final Matrix ma1 = new Matrix(3, 3);
7483 ma1.setSubmatrix(0, 0,
7484 2, 2,
7485 new double[]{sx, myx, mzx,
7486 mxy, sy, mzy,
7487 mxz, myz, sz});
7488 assertEquals(calibrator.getInitialMa(), ma1);
7489 final Matrix ma2 = new Matrix(3, 3);
7490 calibrator.getInitialMa(ma2);
7491 assertEquals(ma1, ma2);
7492 assertNull(calibrator.getMeasurements());
7493 assertTrue(calibrator.isCommonAxisUsed());
7494 assertNull(calibrator.getListener());
7495 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7496 assertFalse(calibrator.isReady());
7497 assertFalse(calibrator.isRunning());
7498 assertNull(calibrator.getEstimatedMa());
7499 assertNull(calibrator.getEstimatedSx());
7500 assertNull(calibrator.getEstimatedSy());
7501 assertNull(calibrator.getEstimatedSz());
7502 assertNull(calibrator.getEstimatedMxy());
7503 assertNull(calibrator.getEstimatedMxz());
7504 assertNull(calibrator.getEstimatedMyx());
7505 assertNull(calibrator.getEstimatedMyz());
7506 assertNull(calibrator.getEstimatedMzx());
7507 assertNull(calibrator.getEstimatedMzy());
7508 assertNull(calibrator.getEstimatedCovariance());
7509 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7510 assertNull(calibrator.getGroundTruthGravityNorm());
7511 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7512 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7513
7514
7515 calibrator = null;
7516 try {
7517 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7518 true, new Matrix(1, 1), ma);
7519 fail("IllegalArgumentException expected but not thrown");
7520 } catch (final IllegalArgumentException ignore) {
7521 }
7522 try {
7523 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7524 true, new Matrix(1, 3), ma);
7525 fail("IllegalArgumentException expected but not thrown");
7526 } catch (final IllegalArgumentException ignore) {
7527 }
7528 try {
7529 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7530 true, ba, new Matrix(1, 3));
7531 fail("IllegalArgumentException expected but not thrown");
7532 } catch (final IllegalArgumentException ignore) {
7533 }
7534 try {
7535 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7536 true, ba, new Matrix(3, 1));
7537 fail("IllegalArgumentException expected but not thrown");
7538 } catch (final IllegalArgumentException ignore) {
7539 }
7540 assertNull(calibrator);
7541 }
7542
7543 @Test
7544 public void testConstructor76() throws WrongSizeException {
7545 final Matrix ba = generateBa();
7546 final double biasX = ba.getElementAtIndex(0);
7547 final double biasY = ba.getElementAtIndex(1);
7548 final double biasZ = ba.getElementAtIndex(2);
7549
7550 final Matrix ma = generateMaCommonAxis();
7551 final double sx = ma.getElementAt(0, 0);
7552 final double sy = ma.getElementAt(1, 1);
7553 final double sz = ma.getElementAt(2, 2);
7554 final double mxy = ma.getElementAt(0, 1);
7555 final double mxz = ma.getElementAt(0, 2);
7556 final double myx = ma.getElementAt(1, 0);
7557 final double myz = ma.getElementAt(1, 2);
7558 final double mzx = ma.getElementAt(2, 0);
7559 final double mzy = ma.getElementAt(2, 1);
7560
7561 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7562 new KnownBiasAndGravityNormAccelerometerCalibrator(
7563 true, ba, ma, this);
7564
7565
7566 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7567 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7568 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7569 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7570 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7571 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7572 final Acceleration bx2 = new Acceleration(0.0,
7573 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7574 calibrator.getBiasXAsAcceleration(bx2);
7575 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7576 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7577 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7578 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7579 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7580 final Acceleration by2 = new Acceleration(0.0,
7581 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7582 calibrator.getBiasYAsAcceleration(by2);
7583 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7584 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7585 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7586 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7587 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7588 final Acceleration bz2 = new Acceleration(0.0,
7589 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7590 calibrator.getBiasZAsAcceleration(bz2);
7591 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7592 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7593 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7594 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7595 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7596 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7597 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7598 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7599 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7600 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7601 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7602 final double[] bias1 = calibrator.getBias();
7603 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7604 final double[] bias2 = new double[3];
7605 calibrator.getBias(bias2);
7606 assertArrayEquals(bias1, bias2, 0.0);
7607 final Matrix b1 = calibrator.getBiasAsMatrix();
7608 assertEquals(b1, ba);
7609 final Matrix b2 = new Matrix(3, 1);
7610 calibrator.getBiasAsMatrix(b2);
7611 assertEquals(b1, b2);
7612 final Matrix ma1 = new Matrix(3, 3);
7613 ma1.setSubmatrix(0, 0,
7614 2, 2,
7615 new double[]{sx, myx, mzx,
7616 mxy, sy, mzy,
7617 mxz, myz, sz});
7618 assertEquals(calibrator.getInitialMa(), ma1);
7619 final Matrix ma2 = new Matrix(3, 3);
7620 calibrator.getInitialMa(ma2);
7621 assertEquals(ma1, ma2);
7622 assertNull(calibrator.getMeasurements());
7623 assertTrue(calibrator.isCommonAxisUsed());
7624 assertSame(calibrator.getListener(), this);
7625 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7626 assertFalse(calibrator.isReady());
7627 assertFalse(calibrator.isRunning());
7628 assertNull(calibrator.getEstimatedMa());
7629 assertNull(calibrator.getEstimatedSx());
7630 assertNull(calibrator.getEstimatedSy());
7631 assertNull(calibrator.getEstimatedSz());
7632 assertNull(calibrator.getEstimatedMxy());
7633 assertNull(calibrator.getEstimatedMxz());
7634 assertNull(calibrator.getEstimatedMyx());
7635 assertNull(calibrator.getEstimatedMyz());
7636 assertNull(calibrator.getEstimatedMzx());
7637 assertNull(calibrator.getEstimatedMzy());
7638 assertNull(calibrator.getEstimatedCovariance());
7639 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7640 assertNull(calibrator.getGroundTruthGravityNorm());
7641 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7642 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7643
7644
7645 calibrator = null;
7646 try {
7647 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7648 true, new Matrix(1, 1), ma,
7649 this);
7650 fail("IllegalArgumentException expected but not thrown");
7651 } catch (final IllegalArgumentException ignore) {
7652 }
7653 try {
7654 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7655 true, new Matrix(1, 3), ma,
7656 this);
7657 fail("IllegalArgumentException expected but not thrown");
7658 } catch (final IllegalArgumentException ignore) {
7659 }
7660 try {
7661 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7662 true, ba, new Matrix(1, 3),
7663 this);
7664 fail("IllegalArgumentException expected but not thrown");
7665 } catch (final IllegalArgumentException ignore) {
7666 }
7667 try {
7668 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7669 true, ba, new Matrix(3, 1),
7670 this);
7671 fail("IllegalArgumentException expected but not thrown");
7672 } catch (final IllegalArgumentException ignore) {
7673 }
7674 assertNull(calibrator);
7675 }
7676
7677 @Test
7678 public void testConstructor77() throws WrongSizeException {
7679 final Collection<StandardDeviationBodyKinematics> measurements =
7680 Collections.emptyList();
7681
7682 final Matrix ba = generateBa();
7683 final double biasX = ba.getElementAtIndex(0);
7684 final double biasY = ba.getElementAtIndex(1);
7685 final double biasZ = ba.getElementAtIndex(2);
7686
7687 final Matrix ma = generateMaCommonAxis();
7688 final double sx = ma.getElementAt(0, 0);
7689 final double sy = ma.getElementAt(1, 1);
7690 final double sz = ma.getElementAt(2, 2);
7691 final double mxy = ma.getElementAt(0, 1);
7692 final double mxz = ma.getElementAt(0, 2);
7693 final double myx = ma.getElementAt(1, 0);
7694 final double myz = ma.getElementAt(1, 2);
7695 final double mzx = ma.getElementAt(2, 0);
7696 final double mzy = ma.getElementAt(2, 1);
7697
7698 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7699 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
7700 true, ba, ma);
7701
7702
7703 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7704 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7705 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7706 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7707 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7708 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7709 final Acceleration bx2 = new Acceleration(0.0,
7710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7711 calibrator.getBiasXAsAcceleration(bx2);
7712 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7713 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7714 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7715 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7716 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7717 final Acceleration by2 = new Acceleration(0.0,
7718 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7719 calibrator.getBiasYAsAcceleration(by2);
7720 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7721 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7722 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7723 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7724 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7725 final Acceleration bz2 = new Acceleration(0.0,
7726 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7727 calibrator.getBiasZAsAcceleration(bz2);
7728 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7729 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7730 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7731 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7732 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7733 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7734 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7735 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7736 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7737 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7738 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7739 final double[] bias1 = calibrator.getBias();
7740 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7741 final double[] bias2 = new double[3];
7742 calibrator.getBias(bias2);
7743 assertArrayEquals(bias1, bias2, 0.0);
7744 final Matrix b1 = calibrator.getBiasAsMatrix();
7745 assertEquals(b1, ba);
7746 final Matrix b2 = new Matrix(3, 1);
7747 calibrator.getBiasAsMatrix(b2);
7748 assertEquals(b1, b2);
7749 final Matrix ma1 = new Matrix(3, 3);
7750 ma1.setSubmatrix(0, 0,
7751 2, 2,
7752 new double[]{sx, myx, mzx,
7753 mxy, sy, mzy,
7754 mxz, myz, sz});
7755 assertEquals(calibrator.getInitialMa(), ma1);
7756 final Matrix ma2 = new Matrix(3, 3);
7757 calibrator.getInitialMa(ma2);
7758 assertEquals(ma1, ma2);
7759 assertSame(calibrator.getMeasurements(), measurements);
7760 assertTrue(calibrator.isCommonAxisUsed());
7761 assertNull(calibrator.getListener());
7762 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7763 assertFalse(calibrator.isReady());
7764 assertFalse(calibrator.isRunning());
7765 assertNull(calibrator.getEstimatedMa());
7766 assertNull(calibrator.getEstimatedSx());
7767 assertNull(calibrator.getEstimatedSy());
7768 assertNull(calibrator.getEstimatedSz());
7769 assertNull(calibrator.getEstimatedMxy());
7770 assertNull(calibrator.getEstimatedMxz());
7771 assertNull(calibrator.getEstimatedMyx());
7772 assertNull(calibrator.getEstimatedMyz());
7773 assertNull(calibrator.getEstimatedMzx());
7774 assertNull(calibrator.getEstimatedMzy());
7775 assertNull(calibrator.getEstimatedCovariance());
7776 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7777 assertNull(calibrator.getGroundTruthGravityNorm());
7778 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7779 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7780
7781
7782 calibrator = null;
7783 try {
7784 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7785 measurements, true,
7786 new Matrix(1, 1), ma);
7787 fail("IllegalArgumentException expected but not thrown");
7788 } catch (final IllegalArgumentException ignore) {
7789 }
7790 try {
7791 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7792 measurements, true,
7793 new Matrix(1, 3), ma);
7794 fail("IllegalArgumentException expected but not thrown");
7795 } catch (final IllegalArgumentException ignore) {
7796 }
7797 try {
7798 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7799 measurements, true, ba,
7800 new Matrix(1, 3));
7801 fail("IllegalArgumentException expected but not thrown");
7802 } catch (final IllegalArgumentException ignore) {
7803 }
7804 try {
7805 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7806 measurements, true, ba,
7807 new Matrix(3, 1));
7808 fail("IllegalArgumentException expected but not thrown");
7809 } catch (final IllegalArgumentException ignore) {
7810 }
7811 assertNull(calibrator);
7812 }
7813
7814 @Test
7815 public void testConstructor78() throws WrongSizeException {
7816 final Collection<StandardDeviationBodyKinematics> measurements =
7817 Collections.emptyList();
7818
7819 final Matrix ba = generateBa();
7820 final double biasX = ba.getElementAtIndex(0);
7821 final double biasY = ba.getElementAtIndex(1);
7822 final double biasZ = ba.getElementAtIndex(2);
7823
7824 final Matrix ma = generateMaCommonAxis();
7825 final double sx = ma.getElementAt(0, 0);
7826 final double sy = ma.getElementAt(1, 1);
7827 final double sz = ma.getElementAt(2, 2);
7828 final double mxy = ma.getElementAt(0, 1);
7829 final double mxz = ma.getElementAt(0, 2);
7830 final double myx = ma.getElementAt(1, 0);
7831 final double myz = ma.getElementAt(1, 2);
7832 final double mzx = ma.getElementAt(2, 0);
7833 final double mzy = ma.getElementAt(2, 1);
7834
7835 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7836 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
7837 true, ba, ma, this);
7838
7839
7840 assertEquals(calibrator.getBiasX(), biasX, 0.0);
7841 assertEquals(calibrator.getBiasY(), biasY, 0.0);
7842 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7843 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7844 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7845 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7846 final Acceleration bx2 = new Acceleration(0.0,
7847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7848 calibrator.getBiasXAsAcceleration(bx2);
7849 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7850 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7851 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7852 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7853 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7854 final Acceleration by2 = new Acceleration(0.0,
7855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7856 calibrator.getBiasYAsAcceleration(by2);
7857 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7858 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7859 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7860 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7861 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7862 final Acceleration bz2 = new Acceleration(0.0,
7863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7864 calibrator.getBiasZAsAcceleration(bz2);
7865 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7866 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7867 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7868 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7869 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7870 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7871 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7872 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7873 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7874 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7875 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7876 final double[] bias1 = calibrator.getBias();
7877 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7878 final double[] bias2 = new double[3];
7879 calibrator.getBias(bias2);
7880 assertArrayEquals(bias1, bias2, 0.0);
7881 final Matrix b1 = calibrator.getBiasAsMatrix();
7882 assertEquals(b1, ba);
7883 final Matrix b2 = new Matrix(3, 1);
7884 calibrator.getBiasAsMatrix(b2);
7885 assertEquals(b1, b2);
7886 final Matrix ma1 = new Matrix(3, 3);
7887 ma1.setSubmatrix(0, 0,
7888 2, 2,
7889 new double[]{sx, myx, mzx,
7890 mxy, sy, mzy,
7891 mxz, myz, sz});
7892 assertEquals(calibrator.getInitialMa(), ma1);
7893 final Matrix ma2 = new Matrix(3, 3);
7894 calibrator.getInitialMa(ma2);
7895 assertEquals(ma1, ma2);
7896 assertSame(calibrator.getMeasurements(), measurements);
7897 assertTrue(calibrator.isCommonAxisUsed());
7898 assertSame(calibrator.getListener(), this);
7899 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7900 assertFalse(calibrator.isReady());
7901 assertFalse(calibrator.isRunning());
7902 assertNull(calibrator.getEstimatedMa());
7903 assertNull(calibrator.getEstimatedSx());
7904 assertNull(calibrator.getEstimatedSy());
7905 assertNull(calibrator.getEstimatedSz());
7906 assertNull(calibrator.getEstimatedMxy());
7907 assertNull(calibrator.getEstimatedMxz());
7908 assertNull(calibrator.getEstimatedMyx());
7909 assertNull(calibrator.getEstimatedMyz());
7910 assertNull(calibrator.getEstimatedMzx());
7911 assertNull(calibrator.getEstimatedMzy());
7912 assertNull(calibrator.getEstimatedCovariance());
7913 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7914 assertNull(calibrator.getGroundTruthGravityNorm());
7915 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7916 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7917
7918
7919 calibrator = null;
7920 try {
7921 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7922 measurements, true,
7923 new Matrix(1, 1), ma,
7924 this);
7925 fail("IllegalArgumentException expected but not thrown");
7926 } catch (final IllegalArgumentException ignore) {
7927 }
7928 try {
7929 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7930 measurements, true,
7931 new Matrix(1, 3), ma,
7932 this);
7933 fail("IllegalArgumentException expected but not thrown");
7934 } catch (final IllegalArgumentException ignore) {
7935 }
7936 try {
7937 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7938 measurements, true, ba, new Matrix(1, 3),
7939 this);
7940 fail("IllegalArgumentException expected but not thrown");
7941 } catch (final IllegalArgumentException ignore) {
7942 }
7943 try {
7944 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7945 measurements, true, ba, new Matrix(3, 1),
7946 this);
7947 fail("IllegalArgumentException expected but not thrown");
7948 } catch (final IllegalArgumentException ignore) {
7949 }
7950 assertNull(calibrator);
7951 }
7952
7953 @Test
7954 public void testConstructor79() throws WrongSizeException {
7955 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
7956 final double latitude = Math.toRadians(
7957 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
7958 final double longitude = Math.toRadians(
7959 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
7960 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
7961 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
7962 final NEDVelocity nedVelocity = new NEDVelocity();
7963 final ECEFPosition ecefPosition = new ECEFPosition();
7964 final ECEFVelocity ecefVelocity = new ECEFVelocity();
7965 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
7966 ecefPosition, ecefVelocity);
7967 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
7968 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
7969 final double gravityNorm = gravity.getNorm();
7970
7971 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7972 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
7973
7974
7975 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
7976 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
7977 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
7978 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7979 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
7980 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7981 final Acceleration bx2 = new Acceleration(0.0,
7982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7983 calibrator.getBiasXAsAcceleration(bx2);
7984 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
7985 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7986 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7987 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
7988 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7989 final Acceleration by2 = new Acceleration(0.0,
7990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7991 calibrator.getBiasYAsAcceleration(by2);
7992 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
7993 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7994 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7995 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
7996 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7997 final Acceleration bz2 = new Acceleration(0.0,
7998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7999 calibrator.getBiasZAsAcceleration(bz2);
8000 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8001 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8002 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8003 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8004 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8005 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8006 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8007 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8008 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8009 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8010 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8011 final double[] bias1 = calibrator.getBias();
8012 assertArrayEquals(bias1, new double[3], 0.0);
8013 final double[] bias2 = new double[3];
8014 calibrator.getBias(bias2);
8015 assertArrayEquals(bias1, bias2, 0.0);
8016 final Matrix b1 = calibrator.getBiasAsMatrix();
8017 assertEquals(b1, new Matrix(3, 1));
8018 final Matrix b2 = new Matrix(3, 1);
8019 calibrator.getBiasAsMatrix(b2);
8020 assertEquals(b1, b2);
8021 final Matrix ma1 = calibrator.getInitialMa();
8022 assertEquals(ma1, new Matrix(3, 3));
8023 final Matrix ma2 = new Matrix(3, 3);
8024 calibrator.getInitialMa(ma2);
8025 assertEquals(ma1, ma2);
8026 assertNull(calibrator.getMeasurements());
8027 assertFalse(calibrator.isCommonAxisUsed());
8028 assertNull(calibrator.getListener());
8029 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8030 assertFalse(calibrator.isReady());
8031 assertFalse(calibrator.isRunning());
8032 assertNull(calibrator.getEstimatedMa());
8033 assertNull(calibrator.getEstimatedSx());
8034 assertNull(calibrator.getEstimatedSy());
8035 assertNull(calibrator.getEstimatedSz());
8036 assertNull(calibrator.getEstimatedMxy());
8037 assertNull(calibrator.getEstimatedMxz());
8038 assertNull(calibrator.getEstimatedMyx());
8039 assertNull(calibrator.getEstimatedMyz());
8040 assertNull(calibrator.getEstimatedMzx());
8041 assertNull(calibrator.getEstimatedMzy());
8042 assertNull(calibrator.getEstimatedCovariance());
8043 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8044 assertNotNull(calibrator.getGroundTruthGravityNorm());
8045 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8046 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8047 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8048 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8049 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8050 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8051 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8052
8053
8054 calibrator = null;
8055 try {
8056 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8057 -gravityNorm);
8058 fail("IllegalArgumentException expected but not thrown");
8059 } catch (final IllegalArgumentException ignore) {
8060 }
8061 assertNull(calibrator);
8062 }
8063
8064 @Test
8065 public void testConstructor80() throws WrongSizeException {
8066 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8067 final double latitude = Math.toRadians(
8068 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8069 final double longitude = Math.toRadians(
8070 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8071 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8072 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8073 final NEDVelocity nedVelocity = new NEDVelocity();
8074 final ECEFPosition ecefPosition = new ECEFPosition();
8075 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8076 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8077 ecefPosition, ecefVelocity);
8078 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8079 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8080 final double gravityNorm = gravity.getNorm();
8081
8082 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8083 new KnownBiasAndGravityNormAccelerometerCalibrator(
8084 gravityNorm, this);
8085
8086
8087 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8088 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8089 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8090 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8091 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8092 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8093 final Acceleration bx2 = new Acceleration(0.0,
8094 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8095 calibrator.getBiasXAsAcceleration(bx2);
8096 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8097 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8098 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8099 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8100 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8101 final Acceleration by2 = new Acceleration(0.0,
8102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8103 calibrator.getBiasYAsAcceleration(by2);
8104 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8105 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8106 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8107 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8108 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8109 final Acceleration bz2 = new Acceleration(0.0,
8110 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8111 calibrator.getBiasZAsAcceleration(bz2);
8112 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8113 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8114 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8115 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8116 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8117 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8118 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8119 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8120 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8121 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8122 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8123 final double[] bias1 = calibrator.getBias();
8124 assertArrayEquals(bias1, new double[3], 0.0);
8125 final double[] bias2 = new double[3];
8126 calibrator.getBias(bias2);
8127 assertArrayEquals(bias1, bias2, 0.0);
8128 final Matrix b1 = calibrator.getBiasAsMatrix();
8129 assertEquals(b1, new Matrix(3, 1));
8130 final Matrix b2 = new Matrix(3, 1);
8131 calibrator.getBiasAsMatrix(b2);
8132 assertEquals(b1, b2);
8133 final Matrix ma1 = calibrator.getInitialMa();
8134 assertEquals(ma1, new Matrix(3, 3));
8135 final Matrix ma2 = new Matrix(3, 3);
8136 calibrator.getInitialMa(ma2);
8137 assertEquals(ma1, ma2);
8138 assertNull(calibrator.getMeasurements());
8139 assertFalse(calibrator.isCommonAxisUsed());
8140 assertSame(calibrator.getListener(), this);
8141 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8142 assertFalse(calibrator.isReady());
8143 assertFalse(calibrator.isRunning());
8144 assertNull(calibrator.getEstimatedMa());
8145 assertNull(calibrator.getEstimatedSx());
8146 assertNull(calibrator.getEstimatedSy());
8147 assertNull(calibrator.getEstimatedSz());
8148 assertNull(calibrator.getEstimatedMxy());
8149 assertNull(calibrator.getEstimatedMxz());
8150 assertNull(calibrator.getEstimatedMyx());
8151 assertNull(calibrator.getEstimatedMyz());
8152 assertNull(calibrator.getEstimatedMzx());
8153 assertNull(calibrator.getEstimatedMzy());
8154 assertNull(calibrator.getEstimatedCovariance());
8155 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8156 assertNotNull(calibrator.getGroundTruthGravityNorm());
8157 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8158 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8159 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8160 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8161 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8162 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8163 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8164
8165
8166 calibrator = null;
8167 try {
8168 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8169 -gravityNorm, this);
8170 fail("IllegalArgumentException expected but not thrown");
8171 } catch (final IllegalArgumentException ignore) {
8172 }
8173 assertNull(calibrator);
8174 }
8175
8176 @Test
8177 public void testConstructor81() throws WrongSizeException {
8178 final Collection<StandardDeviationBodyKinematics> measurements =
8179 Collections.emptyList();
8180
8181 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8182 final double latitude = Math.toRadians(
8183 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8184 final double longitude = Math.toRadians(
8185 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8186 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8187 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8188 final NEDVelocity nedVelocity = new NEDVelocity();
8189 final ECEFPosition ecefPosition = new ECEFPosition();
8190 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8191 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8192 ecefPosition, ecefVelocity);
8193 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8194 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8195 final double gravityNorm = gravity.getNorm();
8196
8197 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8198 new KnownBiasAndGravityNormAccelerometerCalibrator(
8199 gravityNorm, measurements);
8200
8201
8202 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8203 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8204 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8205 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8206 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8207 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8208 final Acceleration bx2 = new Acceleration(0.0,
8209 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8210 calibrator.getBiasXAsAcceleration(bx2);
8211 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8212 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8213 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8214 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8215 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8216 final Acceleration by2 = new Acceleration(0.0,
8217 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8218 calibrator.getBiasYAsAcceleration(by2);
8219 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8220 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8221 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8222 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8223 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8224 final Acceleration bz2 = new Acceleration(0.0,
8225 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8226 calibrator.getBiasZAsAcceleration(bz2);
8227 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8228 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8229 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8230 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8231 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8232 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8233 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8234 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8235 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8236 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8237 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8238 final double[] bias1 = calibrator.getBias();
8239 assertArrayEquals(bias1, new double[3], 0.0);
8240 final double[] bias2 = new double[3];
8241 calibrator.getBias(bias2);
8242 assertArrayEquals(bias1, bias2, 0.0);
8243 final Matrix b1 = calibrator.getBiasAsMatrix();
8244 assertEquals(b1, new Matrix(3, 1));
8245 final Matrix b2 = new Matrix(3, 1);
8246 calibrator.getBiasAsMatrix(b2);
8247 assertEquals(b1, b2);
8248 final Matrix ma1 = calibrator.getInitialMa();
8249 assertEquals(ma1, new Matrix(3, 3));
8250 final Matrix ma2 = new Matrix(3, 3);
8251 calibrator.getInitialMa(ma2);
8252 assertEquals(ma1, ma2);
8253 assertSame(calibrator.getMeasurements(), measurements);
8254 assertFalse(calibrator.isCommonAxisUsed());
8255 assertNull(calibrator.getListener());
8256 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8257 assertFalse(calibrator.isReady());
8258 assertFalse(calibrator.isRunning());
8259 assertNull(calibrator.getEstimatedMa());
8260 assertNull(calibrator.getEstimatedSx());
8261 assertNull(calibrator.getEstimatedSy());
8262 assertNull(calibrator.getEstimatedSz());
8263 assertNull(calibrator.getEstimatedMxy());
8264 assertNull(calibrator.getEstimatedMxz());
8265 assertNull(calibrator.getEstimatedMyx());
8266 assertNull(calibrator.getEstimatedMyz());
8267 assertNull(calibrator.getEstimatedMzx());
8268 assertNull(calibrator.getEstimatedMzy());
8269 assertNull(calibrator.getEstimatedCovariance());
8270 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8271 assertNotNull(calibrator.getGroundTruthGravityNorm());
8272 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8273 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8274 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8275 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8276 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8277 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8278 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8279
8280
8281 calibrator = null;
8282 try {
8283 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8284 -gravityNorm, measurements);
8285 fail("IllegalArgumentException expected but not thrown");
8286 } catch (final IllegalArgumentException ignore) {
8287 }
8288 assertNull(calibrator);
8289 }
8290
8291 @Test
8292 public void testConstructor82() throws WrongSizeException {
8293 final Collection<StandardDeviationBodyKinematics> measurements =
8294 Collections.emptyList();
8295
8296 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8297 final double latitude = Math.toRadians(
8298 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8299 final double longitude = Math.toRadians(
8300 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8301 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8302 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8303 final NEDVelocity nedVelocity = new NEDVelocity();
8304 final ECEFPosition ecefPosition = new ECEFPosition();
8305 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8306 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8307 ecefPosition, ecefVelocity);
8308 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8309 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8310 final double gravityNorm = gravity.getNorm();
8311
8312 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8313 new KnownBiasAndGravityNormAccelerometerCalibrator(
8314 gravityNorm, measurements, this);
8315
8316
8317 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8318 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8319 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8320 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8321 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8322 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8323 final Acceleration bx2 = new Acceleration(0.0,
8324 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8325 calibrator.getBiasXAsAcceleration(bx2);
8326 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8327 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8328 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8329 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8330 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8331 final Acceleration by2 = new Acceleration(0.0,
8332 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8333 calibrator.getBiasYAsAcceleration(by2);
8334 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8335 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8336 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8337 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8338 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8339 final Acceleration bz2 = new Acceleration(0.0,
8340 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8341 calibrator.getBiasZAsAcceleration(bz2);
8342 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8343 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8344 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8345 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8346 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8347 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8348 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8349 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8350 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8351 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8352 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8353 final double[] bias1 = calibrator.getBias();
8354 assertArrayEquals(bias1, new double[3], 0.0);
8355 final double[] bias2 = new double[3];
8356 calibrator.getBias(bias2);
8357 assertArrayEquals(bias1, bias2, 0.0);
8358 final Matrix b1 = calibrator.getBiasAsMatrix();
8359 assertEquals(b1, new Matrix(3, 1));
8360 final Matrix b2 = new Matrix(3, 1);
8361 calibrator.getBiasAsMatrix(b2);
8362 assertEquals(b1, b2);
8363 final Matrix ma1 = calibrator.getInitialMa();
8364 assertEquals(ma1, new Matrix(3, 3));
8365 final Matrix ma2 = new Matrix(3, 3);
8366 calibrator.getInitialMa(ma2);
8367 assertEquals(ma1, ma2);
8368 assertSame(calibrator.getMeasurements(), measurements);
8369 assertFalse(calibrator.isCommonAxisUsed());
8370 assertSame(calibrator.getListener(), this);
8371 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8372 assertFalse(calibrator.isReady());
8373 assertFalse(calibrator.isRunning());
8374 assertNull(calibrator.getEstimatedMa());
8375 assertNull(calibrator.getEstimatedSx());
8376 assertNull(calibrator.getEstimatedSy());
8377 assertNull(calibrator.getEstimatedSz());
8378 assertNull(calibrator.getEstimatedMxy());
8379 assertNull(calibrator.getEstimatedMxz());
8380 assertNull(calibrator.getEstimatedMyx());
8381 assertNull(calibrator.getEstimatedMyz());
8382 assertNull(calibrator.getEstimatedMzx());
8383 assertNull(calibrator.getEstimatedMzy());
8384 assertNull(calibrator.getEstimatedCovariance());
8385 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8386 assertNotNull(calibrator.getGroundTruthGravityNorm());
8387 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8388 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8389 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8390 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8391 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8392 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8393 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8394
8395
8396 calibrator = null;
8397 try {
8398 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8399 -gravityNorm, measurements, this);
8400 fail("IllegalArgumentException expected but not thrown");
8401 } catch (final IllegalArgumentException ignore) {
8402 }
8403 assertNull(calibrator);
8404 }
8405
8406 @Test
8407 public void testConstructor83() throws WrongSizeException {
8408 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8409 final double latitude = Math.toRadians(
8410 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8411 final double longitude = Math.toRadians(
8412 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8413 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8414 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8415 final NEDVelocity nedVelocity = new NEDVelocity();
8416 final ECEFPosition ecefPosition = new ECEFPosition();
8417 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8418 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8419 ecefPosition, ecefVelocity);
8420 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8421 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8422 final double gravityNorm = gravity.getNorm();
8423
8424 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8425 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8426 true);
8427
8428
8429 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8430 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8431 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8432 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8433 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8434 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8435 final Acceleration bx2 = new Acceleration(0.0,
8436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8437 calibrator.getBiasXAsAcceleration(bx2);
8438 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8439 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8440 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8441 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8442 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8443 final Acceleration by2 = new Acceleration(0.0,
8444 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8445 calibrator.getBiasYAsAcceleration(by2);
8446 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8447 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8448 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8449 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8450 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8451 final Acceleration bz2 = new Acceleration(0.0,
8452 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8453 calibrator.getBiasZAsAcceleration(bz2);
8454 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8455 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8456 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8457 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8458 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8459 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8460 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8461 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8462 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8463 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8464 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8465 final double[] bias1 = calibrator.getBias();
8466 assertArrayEquals(bias1, new double[3], 0.0);
8467 final double[] bias2 = new double[3];
8468 calibrator.getBias(bias2);
8469 assertArrayEquals(bias1, bias2, 0.0);
8470 final Matrix b1 = calibrator.getBiasAsMatrix();
8471 assertEquals(b1, new Matrix(3, 1));
8472 final Matrix b2 = new Matrix(3, 1);
8473 calibrator.getBiasAsMatrix(b2);
8474 assertEquals(b1, b2);
8475 final Matrix ma1 = calibrator.getInitialMa();
8476 assertEquals(ma1, new Matrix(3, 3));
8477 final Matrix ma2 = new Matrix(3, 3);
8478 calibrator.getInitialMa(ma2);
8479 assertEquals(ma1, ma2);
8480 assertNull(calibrator.getMeasurements());
8481 assertTrue(calibrator.isCommonAxisUsed());
8482 assertNull(calibrator.getListener());
8483 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8484 assertFalse(calibrator.isReady());
8485 assertFalse(calibrator.isRunning());
8486 assertNull(calibrator.getEstimatedMa());
8487 assertNull(calibrator.getEstimatedSx());
8488 assertNull(calibrator.getEstimatedSy());
8489 assertNull(calibrator.getEstimatedSz());
8490 assertNull(calibrator.getEstimatedMxy());
8491 assertNull(calibrator.getEstimatedMxz());
8492 assertNull(calibrator.getEstimatedMyx());
8493 assertNull(calibrator.getEstimatedMyz());
8494 assertNull(calibrator.getEstimatedMzx());
8495 assertNull(calibrator.getEstimatedMzy());
8496 assertNull(calibrator.getEstimatedCovariance());
8497 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8498 assertNotNull(calibrator.getGroundTruthGravityNorm());
8499 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8500 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8501 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8502 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8503 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8504 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8505 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8506
8507
8508 calibrator = null;
8509 try {
8510 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8511 -gravityNorm, true);
8512 fail("IllegalArgumentException expected but not thrown");
8513 } catch (final IllegalArgumentException ignore) {
8514 }
8515 assertNull(calibrator);
8516 }
8517
8518 @Test
8519 public void testConstructor84() throws WrongSizeException {
8520 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8521 final double latitude = Math.toRadians(
8522 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8523 final double longitude = Math.toRadians(
8524 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8525 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8526 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8527 final NEDVelocity nedVelocity = new NEDVelocity();
8528 final ECEFPosition ecefPosition = new ECEFPosition();
8529 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8530 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8531 ecefPosition, ecefVelocity);
8532 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8533 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8534 final double gravityNorm = gravity.getNorm();
8535
8536 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8537 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8538 true, this);
8539
8540
8541 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8542 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8543 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8544 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8545 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8547 final Acceleration bx2 = new Acceleration(0.0,
8548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8549 calibrator.getBiasXAsAcceleration(bx2);
8550 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8552 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8553 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8555 final Acceleration by2 = new Acceleration(0.0,
8556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8557 calibrator.getBiasYAsAcceleration(by2);
8558 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8560 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8561 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8563 final Acceleration bz2 = new Acceleration(0.0,
8564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8565 calibrator.getBiasZAsAcceleration(bz2);
8566 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8568 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8569 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8570 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8571 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8572 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8573 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8574 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8575 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8576 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8577 final double[] bias1 = calibrator.getBias();
8578 assertArrayEquals(bias1, new double[3], 0.0);
8579 final double[] bias2 = new double[3];
8580 calibrator.getBias(bias2);
8581 assertArrayEquals(bias1, bias2, 0.0);
8582 final Matrix b1 = calibrator.getBiasAsMatrix();
8583 assertEquals(b1, new Matrix(3, 1));
8584 final Matrix b2 = new Matrix(3, 1);
8585 calibrator.getBiasAsMatrix(b2);
8586 assertEquals(b1, b2);
8587 final Matrix ma1 = calibrator.getInitialMa();
8588 assertEquals(ma1, new Matrix(3, 3));
8589 final Matrix ma2 = new Matrix(3, 3);
8590 calibrator.getInitialMa(ma2);
8591 assertEquals(ma1, ma2);
8592 assertNull(calibrator.getMeasurements());
8593 assertTrue(calibrator.isCommonAxisUsed());
8594 assertSame(calibrator.getListener(), this);
8595 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8596 assertFalse(calibrator.isReady());
8597 assertFalse(calibrator.isRunning());
8598 assertNull(calibrator.getEstimatedMa());
8599 assertNull(calibrator.getEstimatedSx());
8600 assertNull(calibrator.getEstimatedSy());
8601 assertNull(calibrator.getEstimatedSz());
8602 assertNull(calibrator.getEstimatedMxy());
8603 assertNull(calibrator.getEstimatedMxz());
8604 assertNull(calibrator.getEstimatedMyx());
8605 assertNull(calibrator.getEstimatedMyz());
8606 assertNull(calibrator.getEstimatedMzx());
8607 assertNull(calibrator.getEstimatedMzy());
8608 assertNull(calibrator.getEstimatedCovariance());
8609 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8610 assertNotNull(calibrator.getGroundTruthGravityNorm());
8611 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8612 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8613 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8614 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8615 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8616 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8617 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8618
8619
8620 calibrator = null;
8621 try {
8622 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8623 -gravityNorm, true, this);
8624 fail("IllegalArgumentException expected but not thrown");
8625 } catch (final IllegalArgumentException ignore) {
8626 }
8627 assertNull(calibrator);
8628 }
8629
8630 @Test
8631 public void testConstructor85() throws WrongSizeException {
8632 final Collection<StandardDeviationBodyKinematics> measurements =
8633 Collections.emptyList();
8634 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8635 final double latitude = Math.toRadians(
8636 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8637 final double longitude = Math.toRadians(
8638 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8639 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8640 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8641 final NEDVelocity nedVelocity = new NEDVelocity();
8642 final ECEFPosition ecefPosition = new ECEFPosition();
8643 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8644 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8645 ecefPosition, ecefVelocity);
8646 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8647 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8648 final double gravityNorm = gravity.getNorm();
8649
8650 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8651 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8652 measurements, true);
8653
8654
8655 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8656 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8657 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8658 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8659 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8660 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8661 final Acceleration bx2 = new Acceleration(0.0,
8662 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8663 calibrator.getBiasXAsAcceleration(bx2);
8664 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8665 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8666 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8667 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8668 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8669 final Acceleration by2 = new Acceleration(0.0,
8670 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8671 calibrator.getBiasYAsAcceleration(by2);
8672 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8673 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8674 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8675 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8676 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8677 final Acceleration bz2 = new Acceleration(0.0,
8678 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8679 calibrator.getBiasZAsAcceleration(bz2);
8680 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8681 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8682 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8683 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8684 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8685 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8686 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8687 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8688 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8689 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8690 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8691 final double[] bias1 = calibrator.getBias();
8692 assertArrayEquals(bias1, new double[3], 0.0);
8693 final double[] bias2 = new double[3];
8694 calibrator.getBias(bias2);
8695 assertArrayEquals(bias1, bias2, 0.0);
8696 final Matrix b1 = calibrator.getBiasAsMatrix();
8697 assertEquals(b1, new Matrix(3, 1));
8698 final Matrix b2 = new Matrix(3, 1);
8699 calibrator.getBiasAsMatrix(b2);
8700 assertEquals(b1, b2);
8701 final Matrix ma1 = calibrator.getInitialMa();
8702 assertEquals(ma1, new Matrix(3, 3));
8703 final Matrix ma2 = new Matrix(3, 3);
8704 calibrator.getInitialMa(ma2);
8705 assertEquals(ma1, ma2);
8706 assertSame(calibrator.getMeasurements(), measurements);
8707 assertTrue(calibrator.isCommonAxisUsed());
8708 assertNull(calibrator.getListener());
8709 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8710 assertFalse(calibrator.isReady());
8711 assertFalse(calibrator.isRunning());
8712 assertNull(calibrator.getEstimatedMa());
8713 assertNull(calibrator.getEstimatedSx());
8714 assertNull(calibrator.getEstimatedSy());
8715 assertNull(calibrator.getEstimatedSz());
8716 assertNull(calibrator.getEstimatedMxy());
8717 assertNull(calibrator.getEstimatedMxz());
8718 assertNull(calibrator.getEstimatedMyx());
8719 assertNull(calibrator.getEstimatedMyz());
8720 assertNull(calibrator.getEstimatedMzx());
8721 assertNull(calibrator.getEstimatedMzy());
8722 assertNull(calibrator.getEstimatedCovariance());
8723 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8724 assertNotNull(calibrator.getGroundTruthGravityNorm());
8725 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8726 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8727 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8728 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8729 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8730 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8731 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8732
8733
8734 calibrator = null;
8735 try {
8736 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8737 -gravityNorm, measurements, true);
8738 fail("IllegalArgumentException expected but not thrown");
8739 } catch (final IllegalArgumentException ignore) {
8740 }
8741 assertNull(calibrator);
8742 }
8743
8744 @Test
8745 public void testConstructor86() throws WrongSizeException {
8746 final Collection<StandardDeviationBodyKinematics> measurements =
8747 Collections.emptyList();
8748 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8749 final double latitude = Math.toRadians(
8750 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8751 final double longitude = Math.toRadians(
8752 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8753 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8754 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8755 final NEDVelocity nedVelocity = new NEDVelocity();
8756 final ECEFPosition ecefPosition = new ECEFPosition();
8757 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8758 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8759 ecefPosition, ecefVelocity);
8760 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8761 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8762 final double gravityNorm = gravity.getNorm();
8763
8764 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8765 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8766 measurements, true, this);
8767
8768
8769 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8770 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8771 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8772 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8773 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8774 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8775 final Acceleration bx2 = new Acceleration(0.0,
8776 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8777 calibrator.getBiasXAsAcceleration(bx2);
8778 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8779 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8780 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8781 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8782 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8783 final Acceleration by2 = new Acceleration(0.0,
8784 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8785 calibrator.getBiasYAsAcceleration(by2);
8786 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8787 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8788 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8789 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8790 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8791 final Acceleration bz2 = new Acceleration(0.0,
8792 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8793 calibrator.getBiasZAsAcceleration(bz2);
8794 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8795 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8796 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8797 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8798 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8799 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8800 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8801 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8802 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8803 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8804 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8805 final double[] bias1 = calibrator.getBias();
8806 assertArrayEquals(bias1, new double[3], 0.0);
8807 final double[] bias2 = new double[3];
8808 calibrator.getBias(bias2);
8809 assertArrayEquals(bias1, bias2, 0.0);
8810 final Matrix b1 = calibrator.getBiasAsMatrix();
8811 assertEquals(b1, new Matrix(3, 1));
8812 final Matrix b2 = new Matrix(3, 1);
8813 calibrator.getBiasAsMatrix(b2);
8814 assertEquals(b1, b2);
8815 final Matrix ma1 = calibrator.getInitialMa();
8816 assertEquals(ma1, new Matrix(3, 3));
8817 final Matrix ma2 = new Matrix(3, 3);
8818 calibrator.getInitialMa(ma2);
8819 assertEquals(ma1, ma2);
8820 assertSame(calibrator.getMeasurements(), measurements);
8821 assertTrue(calibrator.isCommonAxisUsed());
8822 assertSame(calibrator.getListener(), this);
8823 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8824 assertFalse(calibrator.isReady());
8825 assertFalse(calibrator.isRunning());
8826 assertNull(calibrator.getEstimatedMa());
8827 assertNull(calibrator.getEstimatedSx());
8828 assertNull(calibrator.getEstimatedSy());
8829 assertNull(calibrator.getEstimatedSz());
8830 assertNull(calibrator.getEstimatedMxy());
8831 assertNull(calibrator.getEstimatedMxz());
8832 assertNull(calibrator.getEstimatedMyx());
8833 assertNull(calibrator.getEstimatedMyz());
8834 assertNull(calibrator.getEstimatedMzx());
8835 assertNull(calibrator.getEstimatedMzy());
8836 assertNull(calibrator.getEstimatedCovariance());
8837 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8838 assertNotNull(calibrator.getGroundTruthGravityNorm());
8839 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8840 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8841 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8842 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8843 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8844 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8845 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8846
8847
8848 calibrator = null;
8849 try {
8850 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8851 -gravityNorm, measurements, true, this);
8852 fail("IllegalArgumentException expected but not thrown");
8853 } catch (final IllegalArgumentException ignore) {
8854 }
8855 assertNull(calibrator);
8856 }
8857
8858 @Test
8859 public void testConstructor87() throws WrongSizeException {
8860 final Matrix ba = generateBa();
8861 final double biasX = ba.getElementAtIndex(0);
8862 final double biasY = ba.getElementAtIndex(1);
8863 final double biasZ = ba.getElementAtIndex(2);
8864
8865 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8866 final double latitude = Math.toRadians(
8867 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8868 final double longitude = Math.toRadians(
8869 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8870 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8871 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8872 final NEDVelocity nedVelocity = new NEDVelocity();
8873 final ECEFPosition ecefPosition = new ECEFPosition();
8874 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8875 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8876 ecefPosition, ecefVelocity);
8877 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8878 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8879 final double gravityNorm = gravity.getNorm();
8880
8881 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8882 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8883 biasX, biasY, biasZ);
8884
8885
8886 assertEquals(calibrator.getBiasX(), biasX, 0.0);
8887 assertEquals(calibrator.getBiasY(), biasY, 0.0);
8888 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
8889 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8890 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8891 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8892 final Acceleration bx2 = new Acceleration(0.0,
8893 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8894 calibrator.getBiasXAsAcceleration(bx2);
8895 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8896 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8897 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8898 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8899 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8900 final Acceleration by2 = new Acceleration(0.0,
8901 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8902 calibrator.getBiasYAsAcceleration(by2);
8903 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8904 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8905 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8906 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8907 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8908 final Acceleration bz2 = new Acceleration(0.0,
8909 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8910 calibrator.getBiasZAsAcceleration(bz2);
8911 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8912 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8913 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8914 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8915 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8916 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8917 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8918 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8919 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8920 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8921 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8922 final double[] bias1 = calibrator.getBias();
8923 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8924 final double[] bias2 = new double[3];
8925 calibrator.getBias(bias2);
8926 assertArrayEquals(bias1, bias2, 0.0);
8927 final Matrix b1 = calibrator.getBiasAsMatrix();
8928 assertEquals(b1, ba);
8929 final Matrix b2 = new Matrix(3, 1);
8930 calibrator.getBiasAsMatrix(b2);
8931 assertEquals(b1, b2);
8932 final Matrix ma1 = calibrator.getInitialMa();
8933 assertEquals(ma1, new Matrix(3, 3));
8934 final Matrix ma2 = new Matrix(3, 3);
8935 calibrator.getInitialMa(ma2);
8936 assertEquals(ma1, ma2);
8937 assertNull(calibrator.getMeasurements());
8938 assertFalse(calibrator.isCommonAxisUsed());
8939 assertNull(calibrator.getListener());
8940 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8941 assertFalse(calibrator.isReady());
8942 assertFalse(calibrator.isRunning());
8943 assertNull(calibrator.getEstimatedMa());
8944 assertNull(calibrator.getEstimatedSx());
8945 assertNull(calibrator.getEstimatedSy());
8946 assertNull(calibrator.getEstimatedSz());
8947 assertNull(calibrator.getEstimatedMxy());
8948 assertNull(calibrator.getEstimatedMxz());
8949 assertNull(calibrator.getEstimatedMyx());
8950 assertNull(calibrator.getEstimatedMyz());
8951 assertNull(calibrator.getEstimatedMzx());
8952 assertNull(calibrator.getEstimatedMzy());
8953 assertNull(calibrator.getEstimatedCovariance());
8954 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8955 assertNotNull(calibrator.getGroundTruthGravityNorm());
8956 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8957 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8958 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8959 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8960 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8961 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8962 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8963
8964
8965 calibrator = null;
8966 try {
8967 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8968 -gravityNorm, biasX, biasY, biasZ);
8969 fail("IllegalArgumentException expected but not thrown");
8970 } catch (final IllegalArgumentException ignore) {
8971 }
8972 assertNull(calibrator);
8973 }
8974
8975 @Test
8976 public void testConstructor88() throws WrongSizeException {
8977 final Matrix ba = generateBa();
8978 final double biasX = ba.getElementAtIndex(0);
8979 final double biasY = ba.getElementAtIndex(1);
8980 final double biasZ = ba.getElementAtIndex(2);
8981
8982 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8983 final double latitude = Math.toRadians(
8984 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8985 final double longitude = Math.toRadians(
8986 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8987 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8988 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8989 final NEDVelocity nedVelocity = new NEDVelocity();
8990 final ECEFPosition ecefPosition = new ECEFPosition();
8991 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8992 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8993 ecefPosition, ecefVelocity);
8994 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8995 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8996 final double gravityNorm = gravity.getNorm();
8997
8998 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8999 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9000 biasX, biasY, biasZ, this);
9001
9002
9003 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9004 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9005 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9006 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9007 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9008 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9009 final Acceleration bx2 = new Acceleration(0.0,
9010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9011 calibrator.getBiasXAsAcceleration(bx2);
9012 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9013 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9014 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9015 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9016 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9017 final Acceleration by2 = new Acceleration(0.0,
9018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9019 calibrator.getBiasYAsAcceleration(by2);
9020 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9021 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9022 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9023 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9024 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9025 final Acceleration bz2 = new Acceleration(0.0,
9026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9027 calibrator.getBiasZAsAcceleration(bz2);
9028 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9029 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9030 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9031 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9032 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9033 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9034 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9035 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9036 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9037 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9038 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9039 final double[] bias1 = calibrator.getBias();
9040 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9041 final double[] bias2 = new double[3];
9042 calibrator.getBias(bias2);
9043 assertArrayEquals(bias1, bias2, 0.0);
9044 final Matrix b1 = calibrator.getBiasAsMatrix();
9045 assertEquals(b1, ba);
9046 final Matrix b2 = new Matrix(3, 1);
9047 calibrator.getBiasAsMatrix(b2);
9048 assertEquals(b1, b2);
9049 final Matrix ma1 = calibrator.getInitialMa();
9050 assertEquals(ma1, new Matrix(3, 3));
9051 final Matrix ma2 = new Matrix(3, 3);
9052 calibrator.getInitialMa(ma2);
9053 assertEquals(ma1, ma2);
9054 assertNull(calibrator.getMeasurements());
9055 assertFalse(calibrator.isCommonAxisUsed());
9056 assertSame(calibrator.getListener(), this);
9057 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9058 assertFalse(calibrator.isReady());
9059 assertFalse(calibrator.isRunning());
9060 assertNull(calibrator.getEstimatedMa());
9061 assertNull(calibrator.getEstimatedSx());
9062 assertNull(calibrator.getEstimatedSy());
9063 assertNull(calibrator.getEstimatedSz());
9064 assertNull(calibrator.getEstimatedMxy());
9065 assertNull(calibrator.getEstimatedMxz());
9066 assertNull(calibrator.getEstimatedMyx());
9067 assertNull(calibrator.getEstimatedMyz());
9068 assertNull(calibrator.getEstimatedMzx());
9069 assertNull(calibrator.getEstimatedMzy());
9070 assertNull(calibrator.getEstimatedCovariance());
9071 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9072 assertNotNull(calibrator.getGroundTruthGravityNorm());
9073 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9074 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9075 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9076 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9077 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9078 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9079 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9080
9081
9082 calibrator = null;
9083 try {
9084 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9085 -gravityNorm, biasX, biasY, biasZ, this);
9086 fail("IllegalArgumentException expected but not thrown");
9087 } catch (final IllegalArgumentException ignore) {
9088 }
9089 assertNull(calibrator);
9090 }
9091
9092 @Test
9093 public void testConstructor89() throws WrongSizeException {
9094 final Collection<StandardDeviationBodyKinematics> measurements =
9095 Collections.emptyList();
9096
9097 final Matrix ba = generateBa();
9098 final double biasX = ba.getElementAtIndex(0);
9099 final double biasY = ba.getElementAtIndex(1);
9100 final double biasZ = ba.getElementAtIndex(2);
9101
9102 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9103 final double latitude = Math.toRadians(
9104 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9105 final double longitude = Math.toRadians(
9106 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9107 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9108 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9109 final NEDVelocity nedVelocity = new NEDVelocity();
9110 final ECEFPosition ecefPosition = new ECEFPosition();
9111 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9112 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9113 ecefPosition, ecefVelocity);
9114 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9115 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9116 final double gravityNorm = gravity.getNorm();
9117
9118 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9119 new KnownBiasAndGravityNormAccelerometerCalibrator(
9120 gravityNorm, measurements,
9121 biasX, biasY, biasZ);
9122
9123
9124 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9125 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9126 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9127 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9128 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9129 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9130 final Acceleration bx2 = new Acceleration(0.0,
9131 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9132 calibrator.getBiasXAsAcceleration(bx2);
9133 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9134 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9135 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9136 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9137 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9138 final Acceleration by2 = new Acceleration(0.0,
9139 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9140 calibrator.getBiasYAsAcceleration(by2);
9141 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9142 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9143 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9144 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9145 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9146 final Acceleration bz2 = new Acceleration(0.0,
9147 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9148 calibrator.getBiasZAsAcceleration(bz2);
9149 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9150 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9151 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9152 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9153 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9154 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9155 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9156 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9157 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9158 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9159 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9160 final double[] bias1 = calibrator.getBias();
9161 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9162 final double[] bias2 = new double[3];
9163 calibrator.getBias(bias2);
9164 assertArrayEquals(bias1, bias2, 0.0);
9165 final Matrix b1 = calibrator.getBiasAsMatrix();
9166 assertEquals(b1, ba);
9167 final Matrix b2 = new Matrix(3, 1);
9168 calibrator.getBiasAsMatrix(b2);
9169 assertEquals(b1, b2);
9170 final Matrix ma1 = calibrator.getInitialMa();
9171 assertEquals(ma1, new Matrix(3, 3));
9172 final Matrix ma2 = new Matrix(3, 3);
9173 calibrator.getInitialMa(ma2);
9174 assertEquals(ma1, ma2);
9175 assertSame(calibrator.getMeasurements(), measurements);
9176 assertFalse(calibrator.isCommonAxisUsed());
9177 assertNull(calibrator.getListener());
9178 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9179 assertFalse(calibrator.isReady());
9180 assertFalse(calibrator.isRunning());
9181 assertNull(calibrator.getEstimatedMa());
9182 assertNull(calibrator.getEstimatedSx());
9183 assertNull(calibrator.getEstimatedSy());
9184 assertNull(calibrator.getEstimatedSz());
9185 assertNull(calibrator.getEstimatedMxy());
9186 assertNull(calibrator.getEstimatedMxz());
9187 assertNull(calibrator.getEstimatedMyx());
9188 assertNull(calibrator.getEstimatedMyz());
9189 assertNull(calibrator.getEstimatedMzx());
9190 assertNull(calibrator.getEstimatedMzy());
9191 assertNull(calibrator.getEstimatedCovariance());
9192 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9193 assertNotNull(calibrator.getGroundTruthGravityNorm());
9194 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9195 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9196 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9197 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9198 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9199 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9200 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9201
9202
9203 calibrator = null;
9204 try {
9205 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9206 -gravityNorm, measurements,
9207 biasX, biasY, biasZ);
9208 fail("IllegalArgumentException expected but not thrown");
9209 } catch (final IllegalArgumentException ignore) {
9210 }
9211 assertNull(calibrator);
9212 }
9213
9214 @Test
9215 public void testConstructor90() throws WrongSizeException {
9216 final Collection<StandardDeviationBodyKinematics> measurements =
9217 Collections.emptyList();
9218
9219 final Matrix ba = generateBa();
9220 final double biasX = ba.getElementAtIndex(0);
9221 final double biasY = ba.getElementAtIndex(1);
9222 final double biasZ = ba.getElementAtIndex(2);
9223
9224 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9225 final double latitude = Math.toRadians(
9226 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9227 final double longitude = Math.toRadians(
9228 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9229 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9230 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9231 final NEDVelocity nedVelocity = new NEDVelocity();
9232 final ECEFPosition ecefPosition = new ECEFPosition();
9233 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9234 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9235 ecefPosition, ecefVelocity);
9236 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9237 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9238 final double gravityNorm = gravity.getNorm();
9239
9240 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9241 new KnownBiasAndGravityNormAccelerometerCalibrator(
9242 gravityNorm, measurements,
9243 biasX, biasY, biasZ, this);
9244
9245
9246 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9247 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9248 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9249 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9250 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9251 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9252 final Acceleration bx2 = new Acceleration(0.0,
9253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9254 calibrator.getBiasXAsAcceleration(bx2);
9255 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9256 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9257 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9258 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9259 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9260 final Acceleration by2 = new Acceleration(0.0,
9261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9262 calibrator.getBiasYAsAcceleration(by2);
9263 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9264 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9265 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9266 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9267 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9268 final Acceleration bz2 = new Acceleration(0.0,
9269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9270 calibrator.getBiasZAsAcceleration(bz2);
9271 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9272 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9273 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9274 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9275 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9276 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9277 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9278 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9279 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9280 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9281 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9282 final double[] bias1 = calibrator.getBias();
9283 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9284 final double[] bias2 = new double[3];
9285 calibrator.getBias(bias2);
9286 assertArrayEquals(bias1, bias2, 0.0);
9287 final Matrix b1 = calibrator.getBiasAsMatrix();
9288 assertEquals(b1, ba);
9289 final Matrix b2 = new Matrix(3, 1);
9290 calibrator.getBiasAsMatrix(b2);
9291 assertEquals(b1, b2);
9292 final Matrix ma1 = calibrator.getInitialMa();
9293 assertEquals(ma1, new Matrix(3, 3));
9294 final Matrix ma2 = new Matrix(3, 3);
9295 calibrator.getInitialMa(ma2);
9296 assertEquals(ma1, ma2);
9297 assertSame(calibrator.getMeasurements(), measurements);
9298 assertFalse(calibrator.isCommonAxisUsed());
9299 assertSame(calibrator.getListener(), this);
9300 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9301 assertFalse(calibrator.isReady());
9302 assertFalse(calibrator.isRunning());
9303 assertNull(calibrator.getEstimatedMa());
9304 assertNull(calibrator.getEstimatedSx());
9305 assertNull(calibrator.getEstimatedSy());
9306 assertNull(calibrator.getEstimatedSz());
9307 assertNull(calibrator.getEstimatedMxy());
9308 assertNull(calibrator.getEstimatedMxz());
9309 assertNull(calibrator.getEstimatedMyx());
9310 assertNull(calibrator.getEstimatedMyz());
9311 assertNull(calibrator.getEstimatedMzx());
9312 assertNull(calibrator.getEstimatedMzy());
9313 assertNull(calibrator.getEstimatedCovariance());
9314 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9315 assertNotNull(calibrator.getGroundTruthGravityNorm());
9316 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9317 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9318 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9319 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9320 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9321 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9322 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9323
9324
9325 calibrator = null;
9326 try {
9327 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9328 -gravityNorm, measurements,
9329 biasX, biasY, biasZ, this);
9330 fail("IllegalArgumentException expected but not thrown");
9331 } catch (final IllegalArgumentException ignore) {
9332 }
9333 assertNull(calibrator);
9334 }
9335
9336 @Test
9337 public void testConstructor91() throws WrongSizeException {
9338 final Matrix ba = generateBa();
9339 final double biasX = ba.getElementAtIndex(0);
9340 final double biasY = ba.getElementAtIndex(1);
9341 final double biasZ = ba.getElementAtIndex(2);
9342
9343 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9344 final double latitude = Math.toRadians(
9345 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9346 final double longitude = Math.toRadians(
9347 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9348 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9349 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9350 final NEDVelocity nedVelocity = new NEDVelocity();
9351 final ECEFPosition ecefPosition = new ECEFPosition();
9352 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9353 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9354 ecefPosition, ecefVelocity);
9355 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9356 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9357 final double gravityNorm = gravity.getNorm();
9358
9359 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9360 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9361 true, biasX, biasY, biasZ);
9362
9363
9364 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9365 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9366 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9367 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9368 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9369 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9370 final Acceleration bx2 = new Acceleration(0.0,
9371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9372 calibrator.getBiasXAsAcceleration(bx2);
9373 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9374 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9375 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9376 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9377 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9378 final Acceleration by2 = new Acceleration(0.0,
9379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9380 calibrator.getBiasYAsAcceleration(by2);
9381 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9382 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9383 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9384 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9385 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9386 final Acceleration bz2 = new Acceleration(0.0,
9387 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9388 calibrator.getBiasZAsAcceleration(bz2);
9389 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9390 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9391 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9392 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9393 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9394 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9395 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9396 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9397 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9398 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9399 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9400 final double[] bias1 = calibrator.getBias();
9401 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9402 final double[] bias2 = new double[3];
9403 calibrator.getBias(bias2);
9404 assertArrayEquals(bias1, bias2, 0.0);
9405 final Matrix b1 = calibrator.getBiasAsMatrix();
9406 assertEquals(b1, ba);
9407 final Matrix b2 = new Matrix(3, 1);
9408 calibrator.getBiasAsMatrix(b2);
9409 assertEquals(b1, b2);
9410 final Matrix ma1 = calibrator.getInitialMa();
9411 assertEquals(ma1, new Matrix(3, 3));
9412 final Matrix ma2 = new Matrix(3, 3);
9413 calibrator.getInitialMa(ma2);
9414 assertEquals(ma1, ma2);
9415 assertNull(calibrator.getMeasurements());
9416 assertTrue(calibrator.isCommonAxisUsed());
9417 assertNull(calibrator.getListener());
9418 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9419 assertFalse(calibrator.isReady());
9420 assertFalse(calibrator.isRunning());
9421 assertNull(calibrator.getEstimatedMa());
9422 assertNull(calibrator.getEstimatedSx());
9423 assertNull(calibrator.getEstimatedSy());
9424 assertNull(calibrator.getEstimatedSz());
9425 assertNull(calibrator.getEstimatedMxy());
9426 assertNull(calibrator.getEstimatedMxz());
9427 assertNull(calibrator.getEstimatedMyx());
9428 assertNull(calibrator.getEstimatedMyz());
9429 assertNull(calibrator.getEstimatedMzx());
9430 assertNull(calibrator.getEstimatedMzy());
9431 assertNull(calibrator.getEstimatedCovariance());
9432 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9433 assertNotNull(calibrator.getGroundTruthGravityNorm());
9434 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9435 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9436 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9437 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9438 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9439 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9440 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9441
9442
9443 calibrator = null;
9444 try {
9445 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9446 -gravityNorm, true, biasX, biasY, biasZ);
9447 fail("IllegalArgumentException expected but not thrown");
9448 } catch (final IllegalArgumentException ignore) {
9449 }
9450 assertNull(calibrator);
9451 }
9452
9453 @Test
9454 public void testConstructor92() throws WrongSizeException {
9455 final Matrix ba = generateBa();
9456 final double biasX = ba.getElementAtIndex(0);
9457 final double biasY = ba.getElementAtIndex(1);
9458 final double biasZ = ba.getElementAtIndex(2);
9459
9460 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9461 final double latitude = Math.toRadians(
9462 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9463 final double longitude = Math.toRadians(
9464 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9465 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9466 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9467 final NEDVelocity nedVelocity = new NEDVelocity();
9468 final ECEFPosition ecefPosition = new ECEFPosition();
9469 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9470 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9471 ecefPosition, ecefVelocity);
9472 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9473 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9474 final double gravityNorm = gravity.getNorm();
9475
9476 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9477 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9478 true, biasX, biasY, biasZ,
9479 this);
9480
9481
9482 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9483 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9484 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9485 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9486 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9487 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9488 final Acceleration bx2 = new Acceleration(0.0,
9489 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9490 calibrator.getBiasXAsAcceleration(bx2);
9491 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9492 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9493 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9494 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9495 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9496 final Acceleration by2 = new Acceleration(0.0,
9497 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9498 calibrator.getBiasYAsAcceleration(by2);
9499 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9500 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9501 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9502 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9503 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9504 final Acceleration bz2 = new Acceleration(0.0,
9505 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9506 calibrator.getBiasZAsAcceleration(bz2);
9507 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9508 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9509 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9510 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9511 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9512 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9513 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9514 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9515 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9516 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9517 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9518 final double[] bias1 = calibrator.getBias();
9519 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9520 final double[] bias2 = new double[3];
9521 calibrator.getBias(bias2);
9522 assertArrayEquals(bias1, bias2, 0.0);
9523 final Matrix b1 = calibrator.getBiasAsMatrix();
9524 assertEquals(b1, ba);
9525 final Matrix b2 = new Matrix(3, 1);
9526 calibrator.getBiasAsMatrix(b2);
9527 assertEquals(b1, b2);
9528 final Matrix ma1 = calibrator.getInitialMa();
9529 assertEquals(ma1, new Matrix(3, 3));
9530 final Matrix ma2 = new Matrix(3, 3);
9531 calibrator.getInitialMa(ma2);
9532 assertEquals(ma1, ma2);
9533 assertNull(calibrator.getMeasurements());
9534 assertTrue(calibrator.isCommonAxisUsed());
9535 assertSame(calibrator.getListener(), this);
9536 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9537 assertFalse(calibrator.isReady());
9538 assertFalse(calibrator.isRunning());
9539 assertNull(calibrator.getEstimatedMa());
9540 assertNull(calibrator.getEstimatedSx());
9541 assertNull(calibrator.getEstimatedSy());
9542 assertNull(calibrator.getEstimatedSz());
9543 assertNull(calibrator.getEstimatedMxy());
9544 assertNull(calibrator.getEstimatedMxz());
9545 assertNull(calibrator.getEstimatedMyx());
9546 assertNull(calibrator.getEstimatedMyz());
9547 assertNull(calibrator.getEstimatedMzx());
9548 assertNull(calibrator.getEstimatedMzy());
9549 assertNull(calibrator.getEstimatedCovariance());
9550 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9551 assertNotNull(calibrator.getGroundTruthGravityNorm());
9552 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9553 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9554 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9555 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9556 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9557 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9558 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9559
9560
9561 calibrator = null;
9562 try {
9563 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9564 -gravityNorm, true, biasX, biasY, biasZ,
9565 this);
9566 fail("IllegalArgumentException expected but not thrown");
9567 } catch (final IllegalArgumentException ignore) {
9568 }
9569 assertNull(calibrator);
9570 }
9571
9572 @Test
9573 public void testConstructor93() throws WrongSizeException {
9574 final Collection<StandardDeviationBodyKinematics> measurements =
9575 Collections.emptyList();
9576
9577 final Matrix ba = generateBa();
9578 final double biasX = ba.getElementAtIndex(0);
9579 final double biasY = ba.getElementAtIndex(1);
9580 final double biasZ = ba.getElementAtIndex(2);
9581
9582 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9583 final double latitude = Math.toRadians(
9584 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9585 final double longitude = Math.toRadians(
9586 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9587 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9588 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9589 final NEDVelocity nedVelocity = new NEDVelocity();
9590 final ECEFPosition ecefPosition = new ECEFPosition();
9591 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9592 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9593 ecefPosition, ecefVelocity);
9594 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9595 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9596 final double gravityNorm = gravity.getNorm();
9597
9598 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9599 new KnownBiasAndGravityNormAccelerometerCalibrator(
9600 gravityNorm, measurements,
9601 true, biasX, biasY, biasZ);
9602
9603
9604 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9605 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9606 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9607 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9608 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9609 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9610 final Acceleration bx2 = new Acceleration(0.0,
9611 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9612 calibrator.getBiasXAsAcceleration(bx2);
9613 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9614 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9615 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9616 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9617 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9618 final Acceleration by2 = new Acceleration(0.0,
9619 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9620 calibrator.getBiasYAsAcceleration(by2);
9621 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9622 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9623 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9624 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9625 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9626 final Acceleration bz2 = new Acceleration(0.0,
9627 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9628 calibrator.getBiasZAsAcceleration(bz2);
9629 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9630 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9631 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9632 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9633 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9634 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9635 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9636 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9637 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9638 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9639 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9640 final double[] bias1 = calibrator.getBias();
9641 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9642 final double[] bias2 = new double[3];
9643 calibrator.getBias(bias2);
9644 assertArrayEquals(bias1, bias2, 0.0);
9645 final Matrix b1 = calibrator.getBiasAsMatrix();
9646 assertEquals(b1, ba);
9647 final Matrix b2 = new Matrix(3, 1);
9648 calibrator.getBiasAsMatrix(b2);
9649 assertEquals(b1, b2);
9650 final Matrix ma1 = calibrator.getInitialMa();
9651 assertEquals(ma1, new Matrix(3, 3));
9652 final Matrix ma2 = new Matrix(3, 3);
9653 calibrator.getInitialMa(ma2);
9654 assertEquals(ma1, ma2);
9655 assertSame(calibrator.getMeasurements(), measurements);
9656 assertTrue(calibrator.isCommonAxisUsed());
9657 assertNull(calibrator.getListener());
9658 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9659 assertFalse(calibrator.isReady());
9660 assertFalse(calibrator.isRunning());
9661 assertNull(calibrator.getEstimatedMa());
9662 assertNull(calibrator.getEstimatedSx());
9663 assertNull(calibrator.getEstimatedSy());
9664 assertNull(calibrator.getEstimatedSz());
9665 assertNull(calibrator.getEstimatedMxy());
9666 assertNull(calibrator.getEstimatedMxz());
9667 assertNull(calibrator.getEstimatedMyx());
9668 assertNull(calibrator.getEstimatedMyz());
9669 assertNull(calibrator.getEstimatedMzx());
9670 assertNull(calibrator.getEstimatedMzy());
9671 assertNull(calibrator.getEstimatedCovariance());
9672 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9673 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9674 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9675 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9676 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9677 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9678 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9679 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9680
9681
9682 calibrator = null;
9683 try {
9684 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9685 -gravityNorm, measurements,
9686 true, biasX, biasY, biasZ);
9687 fail("IllegalArgumentException expected but not thrown");
9688 } catch (final IllegalArgumentException ignore) {
9689 }
9690 assertNull(calibrator);
9691 }
9692
9693 @Test
9694 public void testConstructor94() throws WrongSizeException {
9695 final Collection<StandardDeviationBodyKinematics> measurements =
9696 Collections.emptyList();
9697
9698 final Matrix ba = generateBa();
9699 final double biasX = ba.getElementAtIndex(0);
9700 final double biasY = ba.getElementAtIndex(1);
9701 final double biasZ = ba.getElementAtIndex(2);
9702
9703 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9704 final double latitude = Math.toRadians(
9705 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9706 final double longitude = Math.toRadians(
9707 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9708 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9709 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9710 final NEDVelocity nedVelocity = new NEDVelocity();
9711 final ECEFPosition ecefPosition = new ECEFPosition();
9712 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9713 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9714 ecefPosition, ecefVelocity);
9715 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9716 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9717 final double gravityNorm = gravity.getNorm();
9718
9719 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9720 new KnownBiasAndGravityNormAccelerometerCalibrator(
9721 gravityNorm, measurements,
9722 true, biasX, biasY, biasZ, this);
9723
9724
9725 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9726 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9727 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9728 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9729 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9730 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9731 final Acceleration bx2 = new Acceleration(0.0,
9732 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9733 calibrator.getBiasXAsAcceleration(bx2);
9734 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9735 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9736 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9737 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9738 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9739 final Acceleration by2 = new Acceleration(0.0,
9740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9741 calibrator.getBiasYAsAcceleration(by2);
9742 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9743 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9744 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9745 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9746 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9747 final Acceleration bz2 = new Acceleration(0.0,
9748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9749 calibrator.getBiasZAsAcceleration(bz2);
9750 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9751 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9752 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9753 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9754 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9755 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9756 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9757 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9758 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9759 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9760 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9761 final double[] bias1 = calibrator.getBias();
9762 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9763 final double[] bias2 = new double[3];
9764 calibrator.getBias(bias2);
9765 assertArrayEquals(bias1, bias2, 0.0);
9766 final Matrix b1 = calibrator.getBiasAsMatrix();
9767 assertEquals(b1, ba);
9768 final Matrix b2 = new Matrix(3, 1);
9769 calibrator.getBiasAsMatrix(b2);
9770 assertEquals(b1, b2);
9771 final Matrix ma1 = calibrator.getInitialMa();
9772 assertEquals(ma1, new Matrix(3, 3));
9773 final Matrix ma2 = new Matrix(3, 3);
9774 calibrator.getInitialMa(ma2);
9775 assertEquals(ma1, ma2);
9776 assertSame(calibrator.getMeasurements(), measurements);
9777 assertTrue(calibrator.isCommonAxisUsed());
9778 assertSame(calibrator.getListener(), this);
9779 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9780 assertFalse(calibrator.isReady());
9781 assertFalse(calibrator.isRunning());
9782 assertNull(calibrator.getEstimatedMa());
9783 assertNull(calibrator.getEstimatedSx());
9784 assertNull(calibrator.getEstimatedSy());
9785 assertNull(calibrator.getEstimatedSz());
9786 assertNull(calibrator.getEstimatedMxy());
9787 assertNull(calibrator.getEstimatedMxz());
9788 assertNull(calibrator.getEstimatedMyx());
9789 assertNull(calibrator.getEstimatedMyz());
9790 assertNull(calibrator.getEstimatedMzx());
9791 assertNull(calibrator.getEstimatedMzy());
9792 assertNull(calibrator.getEstimatedCovariance());
9793 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9794 assertNotNull(calibrator.getGroundTruthGravityNorm());
9795 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9796 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9797 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9798 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9799 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9800 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9801 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9802
9803
9804 calibrator = null;
9805 try {
9806 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9807 -gravityNorm, measurements,
9808 true, biasX, biasY, biasZ, this);
9809 fail("IllegalArgumentException expected but not thrown");
9810 } catch (final IllegalArgumentException ignore) {
9811 }
9812 assertNull(calibrator);
9813 }
9814
9815 @Test
9816 public void testConstructor95() throws WrongSizeException {
9817 final Matrix ba = generateBa();
9818 final double biasX = ba.getElementAtIndex(0);
9819 final double biasY = ba.getElementAtIndex(1);
9820 final double biasZ = ba.getElementAtIndex(2);
9821
9822 final Acceleration bx = new Acceleration(biasX,
9823 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9824 final Acceleration by = new Acceleration(biasY,
9825 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9826 final Acceleration bz = new Acceleration(biasZ,
9827 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9828
9829 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9830 final double latitude = Math.toRadians(
9831 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9832 final double longitude = Math.toRadians(
9833 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9834 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9835 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9836 final NEDVelocity nedVelocity = new NEDVelocity();
9837 final ECEFPosition ecefPosition = new ECEFPosition();
9838 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9839 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9840 ecefPosition, ecefVelocity);
9841 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9842 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9843 final double gravityNorm = gravity.getNorm();
9844
9845 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9846 new KnownBiasAndGravityNormAccelerometerCalibrator(
9847 gravityNorm, bx, by, bz);
9848
9849
9850 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9851 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9852 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9853 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9854 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9855 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9856 final Acceleration bx2 = new Acceleration(0.0,
9857 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9858 calibrator.getBiasXAsAcceleration(bx2);
9859 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9860 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9861 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9862 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9863 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9864 final Acceleration by2 = new Acceleration(0.0,
9865 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9866 calibrator.getBiasYAsAcceleration(by2);
9867 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9868 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9869 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9870 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9871 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9872 final Acceleration bz2 = new Acceleration(0.0,
9873 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9874 calibrator.getBiasZAsAcceleration(bz2);
9875 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9876 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9877 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9878 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9879 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9880 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9881 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9882 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9883 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9884 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9885 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9886 final double[] bias1 = calibrator.getBias();
9887 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9888 final double[] bias2 = new double[3];
9889 calibrator.getBias(bias2);
9890 assertArrayEquals(bias1, bias2, 0.0);
9891 final Matrix b1 = calibrator.getBiasAsMatrix();
9892 assertEquals(b1, ba);
9893 final Matrix b2 = new Matrix(3, 1);
9894 calibrator.getBiasAsMatrix(b2);
9895 assertEquals(b1, b2);
9896 final Matrix ma1 = calibrator.getInitialMa();
9897 assertEquals(ma1, new Matrix(3, 3));
9898 final Matrix ma2 = new Matrix(3, 3);
9899 calibrator.getInitialMa(ma2);
9900 assertEquals(ma1, ma2);
9901 assertNull(calibrator.getMeasurements());
9902 assertFalse(calibrator.isCommonAxisUsed());
9903 assertNull(calibrator.getListener());
9904 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9905 assertFalse(calibrator.isReady());
9906 assertFalse(calibrator.isRunning());
9907 assertNull(calibrator.getEstimatedMa());
9908 assertNull(calibrator.getEstimatedSx());
9909 assertNull(calibrator.getEstimatedSy());
9910 assertNull(calibrator.getEstimatedSz());
9911 assertNull(calibrator.getEstimatedMxy());
9912 assertNull(calibrator.getEstimatedMxz());
9913 assertNull(calibrator.getEstimatedMyx());
9914 assertNull(calibrator.getEstimatedMyz());
9915 assertNull(calibrator.getEstimatedMzx());
9916 assertNull(calibrator.getEstimatedMzy());
9917 assertNull(calibrator.getEstimatedCovariance());
9918 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9919 assertNotNull(calibrator.getGroundTruthGravityNorm());
9920 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9921 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9922 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9923 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9924 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9925 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9926 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9927
9928
9929 calibrator = null;
9930 try {
9931 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9932 -gravityNorm, bx, by, bz);
9933 fail("IllegalArgumentException expected but not thrown");
9934 } catch (final IllegalArgumentException ignore) {
9935 }
9936 assertNull(calibrator);
9937 }
9938
9939 @Test
9940 public void testConstructor96() throws WrongSizeException {
9941 final Matrix ba = generateBa();
9942 final double biasX = ba.getElementAtIndex(0);
9943 final double biasY = ba.getElementAtIndex(1);
9944 final double biasZ = ba.getElementAtIndex(2);
9945
9946 final Acceleration bx = new Acceleration(biasX,
9947 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9948 final Acceleration by = new Acceleration(biasY,
9949 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9950 final Acceleration bz = new Acceleration(biasZ,
9951 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9952
9953 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9954 final double latitude = Math.toRadians(
9955 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9956 final double longitude = Math.toRadians(
9957 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9958 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9959 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9960 final NEDVelocity nedVelocity = new NEDVelocity();
9961 final ECEFPosition ecefPosition = new ECEFPosition();
9962 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9963 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9964 ecefPosition, ecefVelocity);
9965 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9966 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9967 final double gravityNorm = gravity.getNorm();
9968
9969 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9970 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9971 bx, by, bz, this);
9972
9973
9974 assertEquals(calibrator.getBiasX(), biasX, 0.0);
9975 assertEquals(calibrator.getBiasY(), biasY, 0.0);
9976 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9977 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9978 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9979 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9980 final Acceleration bx2 = new Acceleration(0.0,
9981 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9982 calibrator.getBiasXAsAcceleration(bx2);
9983 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9984 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9985 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9986 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9987 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9988 final Acceleration by2 = new Acceleration(0.0,
9989 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9990 calibrator.getBiasYAsAcceleration(by2);
9991 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9992 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9993 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9994 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9995 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9996 final Acceleration bz2 = new Acceleration(0.0,
9997 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9998 calibrator.getBiasZAsAcceleration(bz2);
9999 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10000 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10001 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10002 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10003 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10004 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10005 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10006 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10007 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10008 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10009 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10010 final double[] bias1 = calibrator.getBias();
10011 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10012 final double[] bias2 = new double[3];
10013 calibrator.getBias(bias2);
10014 assertArrayEquals(bias1, bias2, 0.0);
10015 final Matrix b1 = calibrator.getBiasAsMatrix();
10016 assertEquals(b1, ba);
10017 final Matrix b2 = new Matrix(3, 1);
10018 calibrator.getBiasAsMatrix(b2);
10019 assertEquals(b1, b2);
10020 final Matrix ma1 = calibrator.getInitialMa();
10021 assertEquals(ma1, new Matrix(3, 3));
10022 final Matrix ma2 = new Matrix(3, 3);
10023 calibrator.getInitialMa(ma2);
10024 assertEquals(ma1, ma2);
10025 assertNull(calibrator.getMeasurements());
10026 assertFalse(calibrator.isCommonAxisUsed());
10027 assertSame(calibrator.getListener(), this);
10028 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10029 assertFalse(calibrator.isReady());
10030 assertFalse(calibrator.isRunning());
10031 assertNull(calibrator.getEstimatedMa());
10032 assertNull(calibrator.getEstimatedSx());
10033 assertNull(calibrator.getEstimatedSy());
10034 assertNull(calibrator.getEstimatedSz());
10035 assertNull(calibrator.getEstimatedMxy());
10036 assertNull(calibrator.getEstimatedMxz());
10037 assertNull(calibrator.getEstimatedMyx());
10038 assertNull(calibrator.getEstimatedMyz());
10039 assertNull(calibrator.getEstimatedMzx());
10040 assertNull(calibrator.getEstimatedMzy());
10041 assertNull(calibrator.getEstimatedCovariance());
10042 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10043 assertNotNull(calibrator.getGroundTruthGravityNorm());
10044 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10045 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10046 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10047 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10048 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10049 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10050 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10051
10052
10053 calibrator = null;
10054 try {
10055 calibrator =
10056 new KnownBiasAndGravityNormAccelerometerCalibrator(
10057 -gravityNorm, bx, by, bz, this);
10058 fail("IllegalArgumentException but not thrown");
10059 } catch (final IllegalArgumentException ignore) {
10060 }
10061 assertNull(calibrator);
10062 }
10063
10064 @Test
10065 public void testConstructor97() throws WrongSizeException {
10066 final Collection<StandardDeviationBodyKinematics> measurements =
10067 Collections.emptyList();
10068
10069 final Matrix ba = generateBa();
10070 final double biasX = ba.getElementAtIndex(0);
10071 final double biasY = ba.getElementAtIndex(1);
10072 final double biasZ = ba.getElementAtIndex(2);
10073
10074 final Acceleration bx = new Acceleration(biasX,
10075 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10076 final Acceleration by = new Acceleration(biasY,
10077 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10078 final Acceleration bz = new Acceleration(biasZ,
10079 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10080
10081 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10082 final double latitude = Math.toRadians(
10083 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10084 final double longitude = Math.toRadians(
10085 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10086 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10087 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10088 final NEDVelocity nedVelocity = new NEDVelocity();
10089 final ECEFPosition ecefPosition = new ECEFPosition();
10090 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10091 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10092 ecefPosition, ecefVelocity);
10093 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10094 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10095 final double gravityNorm = gravity.getNorm();
10096
10097 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10098 new KnownBiasAndGravityNormAccelerometerCalibrator(
10099 gravityNorm, measurements, bx, by, bz);
10100
10101
10102 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10103 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10104 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10105 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10106 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10107 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10108 final Acceleration bx2 = new Acceleration(0.0,
10109 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10110 calibrator.getBiasXAsAcceleration(bx2);
10111 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10112 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10113 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10114 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10115 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10116 final Acceleration by2 = new Acceleration(0.0,
10117 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10118 calibrator.getBiasYAsAcceleration(by2);
10119 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10120 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10121 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10122 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10123 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10124 final Acceleration bz2 = new Acceleration(0.0,
10125 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10126 calibrator.getBiasZAsAcceleration(bz2);
10127 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10128 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10129 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10130 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10131 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10132 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10133 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10134 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10135 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10136 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10137 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10138 final double[] bias1 = calibrator.getBias();
10139 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10140 final double[] bias2 = new double[3];
10141 calibrator.getBias(bias2);
10142 assertArrayEquals(bias1, bias2, 0.0);
10143 final Matrix b1 = calibrator.getBiasAsMatrix();
10144 assertEquals(b1, ba);
10145 final Matrix b2 = new Matrix(3, 1);
10146 calibrator.getBiasAsMatrix(b2);
10147 assertEquals(b1, b2);
10148 final Matrix ma1 = calibrator.getInitialMa();
10149 assertEquals(ma1, new Matrix(3, 3));
10150 final Matrix ma2 = new Matrix(3, 3);
10151 calibrator.getInitialMa(ma2);
10152 assertEquals(ma1, ma2);
10153 assertSame(calibrator.getMeasurements(), measurements);
10154 assertFalse(calibrator.isCommonAxisUsed());
10155 assertNull(calibrator.getListener());
10156 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10157 assertFalse(calibrator.isReady());
10158 assertFalse(calibrator.isRunning());
10159 assertNull(calibrator.getEstimatedMa());
10160 assertNull(calibrator.getEstimatedSx());
10161 assertNull(calibrator.getEstimatedSy());
10162 assertNull(calibrator.getEstimatedSz());
10163 assertNull(calibrator.getEstimatedMxy());
10164 assertNull(calibrator.getEstimatedMxz());
10165 assertNull(calibrator.getEstimatedMyx());
10166 assertNull(calibrator.getEstimatedMyz());
10167 assertNull(calibrator.getEstimatedMzx());
10168 assertNull(calibrator.getEstimatedMzy());
10169 assertNull(calibrator.getEstimatedCovariance());
10170 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10171 assertNotNull(calibrator.getGroundTruthGravityNorm());
10172 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10173 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10174 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10175 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10176 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10177 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10178 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10179
10180
10181 calibrator = null;
10182 try {
10183 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10184 -gravityNorm, measurements, bx, by, bz);
10185 fail("IllegalArgumentException expected but not thrown");
10186 } catch (final IllegalArgumentException ignore) {
10187 }
10188 assertNull(calibrator);
10189 }
10190
10191 @Test
10192 public void testConstructor98() throws WrongSizeException {
10193 final Collection<StandardDeviationBodyKinematics> measurements =
10194 Collections.emptyList();
10195
10196 final Matrix ba = generateBa();
10197 final double biasX = ba.getElementAtIndex(0);
10198 final double biasY = ba.getElementAtIndex(1);
10199 final double biasZ = ba.getElementAtIndex(2);
10200
10201 final Acceleration bx = new Acceleration(biasX,
10202 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10203 final Acceleration by = new Acceleration(biasY,
10204 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10205 final Acceleration bz = new Acceleration(biasZ,
10206 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10207
10208 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10209 final double latitude = Math.toRadians(
10210 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10211 final double longitude = Math.toRadians(
10212 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10213 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10214 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10215 final NEDVelocity nedVelocity = new NEDVelocity();
10216 final ECEFPosition ecefPosition = new ECEFPosition();
10217 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10218 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10219 ecefPosition, ecefVelocity);
10220 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10221 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10222 final double gravityNorm = gravity.getNorm();
10223
10224 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10225 new KnownBiasAndGravityNormAccelerometerCalibrator(
10226 gravityNorm, measurements,
10227 bx, by, bz, this);
10228
10229
10230 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10231 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10232 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10233 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10234 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10235 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10236 final Acceleration bx2 = new Acceleration(0.0,
10237 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10238 calibrator.getBiasXAsAcceleration(bx2);
10239 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10240 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10241 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10242 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10243 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10244 final Acceleration by2 = new Acceleration(0.0,
10245 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10246 calibrator.getBiasYAsAcceleration(by2);
10247 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10248 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10249 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10250 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10251 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10252 final Acceleration bz2 = new Acceleration(0.0,
10253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10254 calibrator.getBiasZAsAcceleration(bz2);
10255 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10256 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10257 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10258 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10259 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10260 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10261 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10262 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10263 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10264 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10265 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10266 final double[] bias1 = calibrator.getBias();
10267 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10268 final double[] bias2 = new double[3];
10269 calibrator.getBias(bias2);
10270 assertArrayEquals(bias1, bias2, 0.0);
10271 final Matrix b1 = calibrator.getBiasAsMatrix();
10272 assertEquals(b1, ba);
10273 final Matrix b2 = new Matrix(3, 1);
10274 calibrator.getBiasAsMatrix(b2);
10275 assertEquals(b1, b2);
10276 final Matrix ma1 = calibrator.getInitialMa();
10277 assertEquals(ma1, new Matrix(3, 3));
10278 final Matrix ma2 = new Matrix(3, 3);
10279 calibrator.getInitialMa(ma2);
10280 assertEquals(ma1, ma2);
10281 assertSame(calibrator.getMeasurements(), measurements);
10282 assertFalse(calibrator.isCommonAxisUsed());
10283 assertSame(calibrator.getListener(), this);
10284 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10285 assertFalse(calibrator.isReady());
10286 assertFalse(calibrator.isRunning());
10287 assertNull(calibrator.getEstimatedMa());
10288 assertNull(calibrator.getEstimatedSx());
10289 assertNull(calibrator.getEstimatedSy());
10290 assertNull(calibrator.getEstimatedSz());
10291 assertNull(calibrator.getEstimatedMxy());
10292 assertNull(calibrator.getEstimatedMxz());
10293 assertNull(calibrator.getEstimatedMyx());
10294 assertNull(calibrator.getEstimatedMyz());
10295 assertNull(calibrator.getEstimatedMzx());
10296 assertNull(calibrator.getEstimatedMzy());
10297 assertNull(calibrator.getEstimatedCovariance());
10298 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10299 assertNotNull(calibrator.getGroundTruthGravityNorm());
10300 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10301 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10302 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10303 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10304 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10305 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10306 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10307
10308
10309 calibrator = null;
10310 try {
10311 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10312 -gravityNorm, measurements,
10313 bx, by, bz, this);
10314 fail("IllegalArgumentException expected but not thrown");
10315 } catch (final IllegalArgumentException ignore) {
10316 }
10317 assertNull(calibrator);
10318 }
10319
10320 @Test
10321 public void testConstructor99() throws WrongSizeException {
10322 final Matrix ba = generateBa();
10323 final double biasX = ba.getElementAtIndex(0);
10324 final double biasY = ba.getElementAtIndex(1);
10325 final double biasZ = ba.getElementAtIndex(2);
10326
10327 final Acceleration bx = new Acceleration(biasX,
10328 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10329 final Acceleration by = new Acceleration(biasY,
10330 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10331 final Acceleration bz = new Acceleration(biasZ,
10332 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10333
10334 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10335 final double latitude = Math.toRadians(
10336 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10337 final double longitude = Math.toRadians(
10338 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10339 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10340 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10341 final NEDVelocity nedVelocity = new NEDVelocity();
10342 final ECEFPosition ecefPosition = new ECEFPosition();
10343 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10344 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10345 ecefPosition, ecefVelocity);
10346 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10347 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10348 final double gravityNorm = gravity.getNorm();
10349
10350 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10351 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10352 true, bx, by, bz);
10353
10354
10355 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10356 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10357 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10358 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10359 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10360 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10361 final Acceleration bx2 = new Acceleration(0.0,
10362 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10363 calibrator.getBiasXAsAcceleration(bx2);
10364 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10365 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10366 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10367 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10368 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10369 final Acceleration by2 = new Acceleration(0.0,
10370 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10371 calibrator.getBiasYAsAcceleration(by2);
10372 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10373 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10374 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10375 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10376 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10377 final Acceleration bz2 = new Acceleration(0.0,
10378 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10379 calibrator.getBiasZAsAcceleration(bz2);
10380 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10381 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10382 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10383 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10384 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10385 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10386 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10387 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10388 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10389 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10390 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10391 final double[] bias1 = calibrator.getBias();
10392 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10393 final double[] bias2 = new double[3];
10394 calibrator.getBias(bias2);
10395 assertArrayEquals(bias1, bias2, 0.0);
10396 final Matrix b1 = calibrator.getBiasAsMatrix();
10397 assertEquals(b1, ba);
10398 final Matrix b2 = new Matrix(3, 1);
10399 calibrator.getBiasAsMatrix(b2);
10400 assertEquals(b1, b2);
10401 final Matrix ma1 = calibrator.getInitialMa();
10402 assertEquals(ma1, new Matrix(3, 3));
10403 final Matrix ma2 = new Matrix(3, 3);
10404 calibrator.getInitialMa(ma2);
10405 assertEquals(ma1, ma2);
10406 assertNull(calibrator.getMeasurements());
10407 assertTrue(calibrator.isCommonAxisUsed());
10408 assertNull(calibrator.getListener());
10409 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10410 assertFalse(calibrator.isReady());
10411 assertFalse(calibrator.isRunning());
10412 assertNull(calibrator.getEstimatedMa());
10413 assertNull(calibrator.getEstimatedSx());
10414 assertNull(calibrator.getEstimatedSy());
10415 assertNull(calibrator.getEstimatedSz());
10416 assertNull(calibrator.getEstimatedMxy());
10417 assertNull(calibrator.getEstimatedMxz());
10418 assertNull(calibrator.getEstimatedMyx());
10419 assertNull(calibrator.getEstimatedMyz());
10420 assertNull(calibrator.getEstimatedMzx());
10421 assertNull(calibrator.getEstimatedMzy());
10422 assertNull(calibrator.getEstimatedCovariance());
10423 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10424 assertNotNull(calibrator.getGroundTruthGravityNorm());
10425 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10426 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10427 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10428 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10429 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10430 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10431 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10432
10433
10434 calibrator = null;
10435 try {
10436 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10437 -gravityNorm, true, bx, by, bz);
10438 fail("IllegalArgumentException expected but not thrown");
10439 } catch (final IllegalArgumentException ignore) {
10440 }
10441 assertNull(calibrator);
10442 }
10443
10444 @Test
10445 public void testConstructor100() throws WrongSizeException {
10446 final Matrix ba = generateBa();
10447 final double biasX = ba.getElementAtIndex(0);
10448 final double biasY = ba.getElementAtIndex(1);
10449 final double biasZ = ba.getElementAtIndex(2);
10450
10451 final Acceleration bx = new Acceleration(biasX,
10452 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10453 final Acceleration by = new Acceleration(biasY,
10454 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10455 final Acceleration bz = new Acceleration(biasZ,
10456 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10457
10458 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10459 final double latitude = Math.toRadians(
10460 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10461 final double longitude = Math.toRadians(
10462 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10463 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10464 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10465 final NEDVelocity nedVelocity = new NEDVelocity();
10466 final ECEFPosition ecefPosition = new ECEFPosition();
10467 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10468 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10469 ecefPosition, ecefVelocity);
10470 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10471 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10472 final double gravityNorm = gravity.getNorm();
10473
10474 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10475 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10476 true, bx, by, bz, this);
10477
10478
10479 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10480 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10481 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10482 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10483 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10484 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10485 final Acceleration bx2 = new Acceleration(0.0,
10486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10487 calibrator.getBiasXAsAcceleration(bx2);
10488 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10489 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10490 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10491 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10492 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10493 final Acceleration by2 = new Acceleration(0.0,
10494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10495 calibrator.getBiasYAsAcceleration(by2);
10496 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10497 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10498 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10499 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10500 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10501 final Acceleration bz2 = new Acceleration(0.0,
10502 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10503 calibrator.getBiasZAsAcceleration(bz2);
10504 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10505 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10506 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10507 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10508 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10509 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10510 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10511 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10512 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10513 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10514 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10515 final double[] bias1 = calibrator.getBias();
10516 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10517 final double[] bias2 = new double[3];
10518 calibrator.getBias(bias2);
10519 assertArrayEquals(bias1, bias2, 0.0);
10520 final Matrix b1 = calibrator.getBiasAsMatrix();
10521 assertEquals(b1, ba);
10522 final Matrix b2 = new Matrix(3, 1);
10523 calibrator.getBiasAsMatrix(b2);
10524 assertEquals(b1, b2);
10525 final Matrix ma1 = calibrator.getInitialMa();
10526 assertEquals(ma1, new Matrix(3, 3));
10527 final Matrix ma2 = new Matrix(3, 3);
10528 calibrator.getInitialMa(ma2);
10529 assertEquals(ma1, ma2);
10530 assertNull(calibrator.getMeasurements());
10531 assertTrue(calibrator.isCommonAxisUsed());
10532 assertSame(calibrator.getListener(), this);
10533 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10534 assertFalse(calibrator.isReady());
10535 assertFalse(calibrator.isRunning());
10536 assertNull(calibrator.getEstimatedMa());
10537 assertNull(calibrator.getEstimatedSx());
10538 assertNull(calibrator.getEstimatedSy());
10539 assertNull(calibrator.getEstimatedSz());
10540 assertNull(calibrator.getEstimatedMxy());
10541 assertNull(calibrator.getEstimatedMxz());
10542 assertNull(calibrator.getEstimatedMyx());
10543 assertNull(calibrator.getEstimatedMyz());
10544 assertNull(calibrator.getEstimatedMzx());
10545 assertNull(calibrator.getEstimatedMzy());
10546 assertNull(calibrator.getEstimatedCovariance());
10547 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10548 assertNotNull(calibrator.getGroundTruthGravityNorm());
10549 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10550 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10551 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10552 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10553 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10554 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10555 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10556
10557
10558 calibrator = null;
10559 try {
10560 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10561 -gravityNorm, true, bx, by, bz, this);
10562 fail("IllegalArgumentException expected but not thrown");
10563 } catch (final IllegalArgumentException ignore) {
10564 }
10565 assertNull(calibrator);
10566 }
10567
10568 @Test
10569 public void testConstructor101() throws WrongSizeException {
10570 final Collection<StandardDeviationBodyKinematics> measurements =
10571 Collections.emptyList();
10572
10573 final Matrix ba = generateBa();
10574 final double biasX = ba.getElementAtIndex(0);
10575 final double biasY = ba.getElementAtIndex(1);
10576 final double biasZ = ba.getElementAtIndex(2);
10577
10578 final Acceleration bx = new Acceleration(biasX,
10579 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10580 final Acceleration by = new Acceleration(biasY,
10581 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10582 final Acceleration bz = new Acceleration(biasZ,
10583 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10584
10585 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10586 final double latitude = Math.toRadians(
10587 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10588 final double longitude = Math.toRadians(
10589 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10590 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10591 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10592 final NEDVelocity nedVelocity = new NEDVelocity();
10593 final ECEFPosition ecefPosition = new ECEFPosition();
10594 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10595 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10596 ecefPosition, ecefVelocity);
10597 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10598 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10599 final double gravityNorm = gravity.getNorm();
10600
10601 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10602 new KnownBiasAndGravityNormAccelerometerCalibrator(
10603 gravityNorm, measurements,
10604 true, bx, by, bz);
10605
10606
10607 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10608 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10609 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10610 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10611 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10612 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10613 final Acceleration bx2 = new Acceleration(0.0,
10614 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10615 calibrator.getBiasXAsAcceleration(bx2);
10616 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10617 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10618 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10619 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10620 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10621 final Acceleration by2 = new Acceleration(0.0,
10622 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10623 calibrator.getBiasYAsAcceleration(by2);
10624 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10625 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10626 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10627 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10628 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10629 final Acceleration bz2 = new Acceleration(0.0,
10630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10631 calibrator.getBiasZAsAcceleration(bz2);
10632 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10633 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10634 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10635 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10636 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10637 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10638 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10639 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10640 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10641 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10642 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10643 final double[] bias1 = calibrator.getBias();
10644 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10645 final double[] bias2 = new double[3];
10646 calibrator.getBias(bias2);
10647 assertArrayEquals(bias1, bias2, 0.0);
10648 final Matrix b1 = calibrator.getBiasAsMatrix();
10649 assertEquals(b1, ba);
10650 final Matrix b2 = new Matrix(3, 1);
10651 calibrator.getBiasAsMatrix(b2);
10652 assertEquals(b1, b2);
10653 final Matrix ma1 = calibrator.getInitialMa();
10654 assertEquals(ma1, new Matrix(3, 3));
10655 final Matrix ma2 = new Matrix(3, 3);
10656 calibrator.getInitialMa(ma2);
10657 assertEquals(ma1, ma2);
10658 assertSame(calibrator.getMeasurements(), measurements);
10659 assertTrue(calibrator.isCommonAxisUsed());
10660 assertNull(calibrator.getListener());
10661 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10662 assertFalse(calibrator.isReady());
10663 assertFalse(calibrator.isRunning());
10664 assertNull(calibrator.getEstimatedMa());
10665 assertNull(calibrator.getEstimatedSx());
10666 assertNull(calibrator.getEstimatedSy());
10667 assertNull(calibrator.getEstimatedSz());
10668 assertNull(calibrator.getEstimatedMxy());
10669 assertNull(calibrator.getEstimatedMxz());
10670 assertNull(calibrator.getEstimatedMyx());
10671 assertNull(calibrator.getEstimatedMyz());
10672 assertNull(calibrator.getEstimatedMzx());
10673 assertNull(calibrator.getEstimatedMzy());
10674 assertNull(calibrator.getEstimatedCovariance());
10675 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10676 assertNotNull(calibrator.getGroundTruthGravityNorm());
10677 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10678 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10679 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10680 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10681 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10682 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10683 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10684
10685
10686 calibrator = null;
10687 try {
10688 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10689 -gravityNorm, measurements,
10690 true, bx, by, bz);
10691 fail("IllegalArgumentException expected but not thrown");
10692 } catch (final IllegalArgumentException ignore) {
10693 }
10694 assertNull(calibrator);
10695 }
10696
10697 @Test
10698 public void testConstructor102() throws WrongSizeException {
10699 final Collection<StandardDeviationBodyKinematics> measurements =
10700 Collections.emptyList();
10701
10702 final Matrix ba = generateBa();
10703 final double biasX = ba.getElementAtIndex(0);
10704 final double biasY = ba.getElementAtIndex(1);
10705 final double biasZ = ba.getElementAtIndex(2);
10706
10707 final Acceleration bx = new Acceleration(biasX,
10708 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10709 final Acceleration by = new Acceleration(biasY,
10710 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10711 final Acceleration bz = new Acceleration(biasZ,
10712 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10713
10714 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10715 final double latitude = Math.toRadians(
10716 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10717 final double longitude = Math.toRadians(
10718 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10719 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10720 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10721 final NEDVelocity nedVelocity = new NEDVelocity();
10722 final ECEFPosition ecefPosition = new ECEFPosition();
10723 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10724 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10725 ecefPosition, ecefVelocity);
10726 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10727 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10728 final double gravityNorm = gravity.getNorm();
10729
10730 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10731 new KnownBiasAndGravityNormAccelerometerCalibrator(
10732 gravityNorm, measurements,
10733 true, bx, by, bz, this);
10734
10735
10736 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10737 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10738 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10739 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10740 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10741 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10742 final Acceleration bx2 = new Acceleration(0.0,
10743 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10744 calibrator.getBiasXAsAcceleration(bx2);
10745 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10746 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10747 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10748 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10749 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10750 final Acceleration by2 = new Acceleration(0.0,
10751 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10752 calibrator.getBiasYAsAcceleration(by2);
10753 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10754 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10755 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10756 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10757 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10758 final Acceleration bz2 = new Acceleration(0.0,
10759 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10760 calibrator.getBiasZAsAcceleration(bz2);
10761 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10762 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10763 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10764 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10765 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10766 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10767 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10768 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10769 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10770 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10771 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10772 final double[] bias1 = calibrator.getBias();
10773 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10774 final double[] bias2 = new double[3];
10775 calibrator.getBias(bias2);
10776 assertArrayEquals(bias1, bias2, 0.0);
10777 final Matrix b1 = calibrator.getBiasAsMatrix();
10778 assertEquals(b1, ba);
10779 final Matrix b2 = new Matrix(3, 1);
10780 calibrator.getBiasAsMatrix(b2);
10781 assertEquals(b1, b2);
10782 final Matrix ma1 = calibrator.getInitialMa();
10783 assertEquals(ma1, new Matrix(3, 3));
10784 final Matrix ma2 = new Matrix(3, 3);
10785 calibrator.getInitialMa(ma2);
10786 assertEquals(ma1, ma2);
10787 assertSame(calibrator.getMeasurements(), measurements);
10788 assertTrue(calibrator.isCommonAxisUsed());
10789 assertSame(calibrator.getListener(), this);
10790 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10791 assertFalse(calibrator.isReady());
10792 assertFalse(calibrator.isRunning());
10793 assertNull(calibrator.getEstimatedMa());
10794 assertNull(calibrator.getEstimatedSx());
10795 assertNull(calibrator.getEstimatedSy());
10796 assertNull(calibrator.getEstimatedSz());
10797 assertNull(calibrator.getEstimatedMxy());
10798 assertNull(calibrator.getEstimatedMxz());
10799 assertNull(calibrator.getEstimatedMyx());
10800 assertNull(calibrator.getEstimatedMyz());
10801 assertNull(calibrator.getEstimatedMzx());
10802 assertNull(calibrator.getEstimatedMzy());
10803 assertNull(calibrator.getEstimatedCovariance());
10804 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10805 assertNotNull(calibrator.getGroundTruthGravityNorm());
10806 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10807 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10808 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10809 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10810 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10811 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10812 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10813
10814
10815 calibrator = null;
10816 try {
10817 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10818 -gravityNorm, measurements,
10819 true, bx, by, bz, this);
10820 fail("IllegalArgumentException expected but not thrown");
10821 } catch (final IllegalArgumentException ignore) {
10822 }
10823 assertNull(calibrator);
10824 }
10825
10826 @Test
10827 public void testConstructor103() throws WrongSizeException {
10828 final Matrix ba = generateBa();
10829 final double biasX = ba.getElementAtIndex(0);
10830 final double biasY = ba.getElementAtIndex(1);
10831 final double biasZ = ba.getElementAtIndex(2);
10832
10833 final Matrix ma = generateMaCommonAxis();
10834 final double sx = ma.getElementAt(0, 0);
10835 final double sy = ma.getElementAt(1, 1);
10836 final double sz = ma.getElementAt(2, 2);
10837
10838 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10839 final double latitude = Math.toRadians(
10840 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10841 final double longitude = Math.toRadians(
10842 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10843 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10844 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10845 final NEDVelocity nedVelocity = new NEDVelocity();
10846 final ECEFPosition ecefPosition = new ECEFPosition();
10847 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10848 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10849 ecefPosition, ecefVelocity);
10850 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10851 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10852 final double gravityNorm = gravity.getNorm();
10853
10854 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10855 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10856 biasX, biasY, biasZ, sx, sy, sz);
10857
10858
10859 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10860 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10861 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10862 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10863 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10864 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10865 final Acceleration bx2 = new Acceleration(0.0,
10866 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10867 calibrator.getBiasXAsAcceleration(bx2);
10868 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10869 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10870 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10871 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10872 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10873 final Acceleration by2 = new Acceleration(0.0,
10874 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10875 calibrator.getBiasYAsAcceleration(by2);
10876 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10877 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10878 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10879 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10880 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10881 final Acceleration bz2 = new Acceleration(0.0,
10882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10883 calibrator.getBiasZAsAcceleration(bz2);
10884 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10885 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10886 assertEquals(calibrator.getInitialSx(), sx, 0.0);
10887 assertEquals(calibrator.getInitialSy(), sy, 0.0);
10888 assertEquals(calibrator.getInitialSz(), sz, 0.0);
10889 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10890 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10891 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10892 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10893 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10894 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10895 final double[] bias1 = calibrator.getBias();
10896 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10897 final double[] bias2 = new double[3];
10898 calibrator.getBias(bias2);
10899 assertArrayEquals(bias1, bias2, 0.0);
10900 final Matrix b1 = calibrator.getBiasAsMatrix();
10901 assertEquals(b1, ba);
10902 final Matrix b2 = new Matrix(3, 1);
10903 calibrator.getBiasAsMatrix(b2);
10904 assertEquals(b1, b2);
10905 final Matrix ma1 = calibrator.getInitialMa();
10906 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
10907 final Matrix ma2 = new Matrix(3, 3);
10908 calibrator.getInitialMa(ma2);
10909 assertEquals(ma1, ma2);
10910 assertNull(calibrator.getMeasurements());
10911 assertFalse(calibrator.isCommonAxisUsed());
10912 assertNull(calibrator.getListener());
10913 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10914 assertFalse(calibrator.isReady());
10915 assertFalse(calibrator.isRunning());
10916 assertNull(calibrator.getEstimatedMa());
10917 assertNull(calibrator.getEstimatedSx());
10918 assertNull(calibrator.getEstimatedSy());
10919 assertNull(calibrator.getEstimatedSz());
10920 assertNull(calibrator.getEstimatedMxy());
10921 assertNull(calibrator.getEstimatedMxz());
10922 assertNull(calibrator.getEstimatedMyx());
10923 assertNull(calibrator.getEstimatedMyz());
10924 assertNull(calibrator.getEstimatedMzx());
10925 assertNull(calibrator.getEstimatedMzy());
10926 assertNull(calibrator.getEstimatedCovariance());
10927 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10928 assertNotNull(calibrator.getGroundTruthGravityNorm());
10929 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10930 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10931 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10932 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10933 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10934 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10935 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10936
10937
10938 calibrator = null;
10939 try {
10940 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10941 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz);
10942 fail("IllegalArgumentException expected but not thrown");
10943 } catch (final IllegalArgumentException ignore) {
10944 }
10945 assertNull(calibrator);
10946 }
10947
10948 @Test
10949 public void testConstructor104() throws WrongSizeException {
10950 final Collection<StandardDeviationBodyKinematics> measurements =
10951 Collections.emptyList();
10952
10953 final Matrix ba = generateBa();
10954 final double biasX = ba.getElementAtIndex(0);
10955 final double biasY = ba.getElementAtIndex(1);
10956 final double biasZ = ba.getElementAtIndex(2);
10957
10958 final Matrix ma = generateMaCommonAxis();
10959 final double sx = ma.getElementAt(0, 0);
10960 final double sy = ma.getElementAt(1, 1);
10961 final double sz = ma.getElementAt(2, 2);
10962
10963 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10964 final double latitude = Math.toRadians(
10965 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10966 final double longitude = Math.toRadians(
10967 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10968 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10969 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10970 final NEDVelocity nedVelocity = new NEDVelocity();
10971 final ECEFPosition ecefPosition = new ECEFPosition();
10972 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10973 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10974 ecefPosition, ecefVelocity);
10975 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10976 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10977 final double gravityNorm = gravity.getNorm();
10978
10979 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10980 new KnownBiasAndGravityNormAccelerometerCalibrator(
10981 gravityNorm, measurements,
10982 biasX, biasY, biasZ, sx, sy, sz);
10983
10984
10985 assertEquals(calibrator.getBiasX(), biasX, 0.0);
10986 assertEquals(calibrator.getBiasY(), biasY, 0.0);
10987 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10988 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10989 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10990 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10991 final Acceleration bx2 = new Acceleration(0.0,
10992 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10993 calibrator.getBiasXAsAcceleration(bx2);
10994 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10995 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10996 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10997 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10998 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10999 final Acceleration by2 = new Acceleration(0.0,
11000 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11001 calibrator.getBiasYAsAcceleration(by2);
11002 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11003 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11004 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11005 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11006 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11007 final Acceleration bz2 = new Acceleration(0.0,
11008 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11009 calibrator.getBiasZAsAcceleration(bz2);
11010 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11011 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11012 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11013 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11014 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11015 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11016 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11017 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11018 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11019 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11020 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11021 final double[] bias1 = calibrator.getBias();
11022 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11023 final double[] bias2 = new double[3];
11024 calibrator.getBias(bias2);
11025 assertArrayEquals(bias1, bias2, 0.0);
11026 final Matrix b1 = calibrator.getBiasAsMatrix();
11027 assertEquals(b1, ba);
11028 final Matrix b2 = new Matrix(3, 1);
11029 calibrator.getBiasAsMatrix(b2);
11030 assertEquals(b1, b2);
11031 final Matrix ma1 = calibrator.getInitialMa();
11032 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11033 final Matrix ma2 = new Matrix(3, 3);
11034 calibrator.getInitialMa(ma2);
11035 assertEquals(ma1, ma2);
11036 assertSame(calibrator.getMeasurements(), measurements);
11037 assertFalse(calibrator.isCommonAxisUsed());
11038 assertNull(calibrator.getListener());
11039 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11040 assertFalse(calibrator.isReady());
11041 assertFalse(calibrator.isRunning());
11042 assertNull(calibrator.getEstimatedMa());
11043 assertNull(calibrator.getEstimatedSx());
11044 assertNull(calibrator.getEstimatedSy());
11045 assertNull(calibrator.getEstimatedSz());
11046 assertNull(calibrator.getEstimatedMxy());
11047 assertNull(calibrator.getEstimatedMxz());
11048 assertNull(calibrator.getEstimatedMyx());
11049 assertNull(calibrator.getEstimatedMyz());
11050 assertNull(calibrator.getEstimatedMzx());
11051 assertNull(calibrator.getEstimatedMzy());
11052 assertNull(calibrator.getEstimatedCovariance());
11053 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11054 assertNotNull(calibrator.getGroundTruthGravityNorm());
11055 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11056 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11057 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11058 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11059 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11060 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11061 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11062
11063
11064 calibrator = null;
11065 try {
11066 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11067 -gravityNorm, measurements,
11068 biasX, biasY, biasZ, sx, sy, sz);
11069 fail("IllegalArgumentException expected but not thrown");
11070 } catch (final IllegalArgumentException ignore) {
11071 }
11072 assertNull(calibrator);
11073 }
11074
11075 @Test
11076 public void testConstructor105() throws WrongSizeException {
11077 final Collection<StandardDeviationBodyKinematics> measurements =
11078 Collections.emptyList();
11079
11080 final Matrix ba = generateBa();
11081 final double biasX = ba.getElementAtIndex(0);
11082 final double biasY = ba.getElementAtIndex(1);
11083 final double biasZ = ba.getElementAtIndex(2);
11084
11085 final Matrix ma = generateMaCommonAxis();
11086 final double sx = ma.getElementAt(0, 0);
11087 final double sy = ma.getElementAt(1, 1);
11088 final double sz = ma.getElementAt(2, 2);
11089
11090 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11091 final double latitude = Math.toRadians(
11092 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11093 final double longitude = Math.toRadians(
11094 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11095 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11096 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11097 final NEDVelocity nedVelocity = new NEDVelocity();
11098 final ECEFPosition ecefPosition = new ECEFPosition();
11099 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11100 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11101 ecefPosition, ecefVelocity);
11102 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11103 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11104 final double gravityNorm = gravity.getNorm();
11105
11106 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11107 new KnownBiasAndGravityNormAccelerometerCalibrator(
11108 gravityNorm, measurements,
11109 biasX, biasY, biasZ, sx, sy, sz, this);
11110
11111
11112 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11113 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11114 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11115 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11116 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11117 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11118 final Acceleration bx2 = new Acceleration(0.0,
11119 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11120 calibrator.getBiasXAsAcceleration(bx2);
11121 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11122 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11123 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11124 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11125 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126 final Acceleration by2 = new Acceleration(0.0,
11127 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11128 calibrator.getBiasYAsAcceleration(by2);
11129 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11130 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11131 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11132 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11133 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11134 final Acceleration bz2 = new Acceleration(0.0,
11135 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11136 calibrator.getBiasZAsAcceleration(bz2);
11137 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11138 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11139 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11140 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11141 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11142 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11143 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11144 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11145 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11146 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11147 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11148 final double[] bias1 = calibrator.getBias();
11149 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11150 final double[] bias2 = new double[3];
11151 calibrator.getBias(bias2);
11152 assertArrayEquals(bias1, bias2, 0.0);
11153 final Matrix b1 = calibrator.getBiasAsMatrix();
11154 assertEquals(b1, ba);
11155 final Matrix b2 = new Matrix(3, 1);
11156 calibrator.getBiasAsMatrix(b2);
11157 assertEquals(b1, b2);
11158 final Matrix ma1 = calibrator.getInitialMa();
11159 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11160 final Matrix ma2 = new Matrix(3, 3);
11161 calibrator.getInitialMa(ma2);
11162 assertEquals(ma1, ma2);
11163 assertSame(calibrator.getMeasurements(), measurements);
11164 assertFalse(calibrator.isCommonAxisUsed());
11165 assertSame(calibrator.getListener(), this);
11166 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11167 assertFalse(calibrator.isReady());
11168 assertFalse(calibrator.isRunning());
11169 assertNull(calibrator.getEstimatedMa());
11170 assertNull(calibrator.getEstimatedSx());
11171 assertNull(calibrator.getEstimatedSy());
11172 assertNull(calibrator.getEstimatedSz());
11173 assertNull(calibrator.getEstimatedMxy());
11174 assertNull(calibrator.getEstimatedMxz());
11175 assertNull(calibrator.getEstimatedMyx());
11176 assertNull(calibrator.getEstimatedMyz());
11177 assertNull(calibrator.getEstimatedMzx());
11178 assertNull(calibrator.getEstimatedMzy());
11179 assertNull(calibrator.getEstimatedCovariance());
11180 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11181 assertNotNull(calibrator.getGroundTruthGravityNorm());
11182 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11183 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11184 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11185 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11186 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11187 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11188 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11189
11190
11191 calibrator = null;
11192 try {
11193 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11194 -gravityNorm, measurements,
11195 biasX, biasY, biasZ, sx, sy, sz, this);
11196 fail("IllegalArgumentException expected but not thrown");
11197 } catch (final IllegalArgumentException ignore) {
11198 }
11199 assertNull(calibrator);
11200 }
11201
11202 @Test
11203 public void testConstructor106() throws WrongSizeException {
11204 final Matrix ba = generateBa();
11205 final double biasX = ba.getElementAtIndex(0);
11206 final double biasY = ba.getElementAtIndex(1);
11207 final double biasZ = ba.getElementAtIndex(2);
11208
11209 final Matrix ma = generateMaCommonAxis();
11210 final double sx = ma.getElementAt(0, 0);
11211 final double sy = ma.getElementAt(1, 1);
11212 final double sz = ma.getElementAt(2, 2);
11213
11214 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11215 final double latitude = Math.toRadians(
11216 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11217 final double longitude = Math.toRadians(
11218 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11219 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11220 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11221 final NEDVelocity nedVelocity = new NEDVelocity();
11222 final ECEFPosition ecefPosition = new ECEFPosition();
11223 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11224 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11225 ecefPosition, ecefVelocity);
11226 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11227 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11228 final double gravityNorm = gravity.getNorm();
11229
11230 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11231 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11232 true, biasX, biasY, biasZ,
11233 sx, sy, sz);
11234
11235
11236 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11237 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11238 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11239 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11240 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11241 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11242 final Acceleration bx2 = new Acceleration(0.0,
11243 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11244 calibrator.getBiasXAsAcceleration(bx2);
11245 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11246 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11247 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11248 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11249 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11250 final Acceleration by2 = new Acceleration(0.0,
11251 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11252 calibrator.getBiasYAsAcceleration(by2);
11253 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11254 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11255 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11256 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11257 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11258 final Acceleration bz2 = new Acceleration(0.0,
11259 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11260 calibrator.getBiasZAsAcceleration(bz2);
11261 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11262 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11263 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11264 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11265 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11266 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11267 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11268 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11269 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11270 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11271 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11272 final double[] bias1 = calibrator.getBias();
11273 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11274 final double[] bias2 = new double[3];
11275 calibrator.getBias(bias2);
11276 assertArrayEquals(bias1, bias2, 0.0);
11277 final Matrix b1 = calibrator.getBiasAsMatrix();
11278 assertEquals(b1, ba);
11279 final Matrix b2 = new Matrix(3, 1);
11280 calibrator.getBiasAsMatrix(b2);
11281 assertEquals(b1, b2);
11282 final Matrix ma1 = calibrator.getInitialMa();
11283 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11284 final Matrix ma2 = new Matrix(3, 3);
11285 calibrator.getInitialMa(ma2);
11286 assertEquals(ma1, ma2);
11287 assertNull(calibrator.getMeasurements());
11288 assertTrue(calibrator.isCommonAxisUsed());
11289 assertNull(calibrator.getListener());
11290 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11291 assertFalse(calibrator.isReady());
11292 assertFalse(calibrator.isRunning());
11293 assertNull(calibrator.getEstimatedMa());
11294 assertNull(calibrator.getEstimatedSx());
11295 assertNull(calibrator.getEstimatedSy());
11296 assertNull(calibrator.getEstimatedSz());
11297 assertNull(calibrator.getEstimatedMxy());
11298 assertNull(calibrator.getEstimatedMxz());
11299 assertNull(calibrator.getEstimatedMyx());
11300 assertNull(calibrator.getEstimatedMyz());
11301 assertNull(calibrator.getEstimatedMzx());
11302 assertNull(calibrator.getEstimatedMzy());
11303 assertNull(calibrator.getEstimatedCovariance());
11304 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11305 assertNotNull(calibrator.getGroundTruthGravityNorm());
11306 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11307 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11308 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11309 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11310 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11311 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11312 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11313
11314
11315 calibrator = null;
11316 try {
11317 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11318 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz);
11319 fail("IllegalArgumentException expected but not thrown");
11320 } catch (final IllegalArgumentException ignore) {
11321 }
11322 assertNull(calibrator);
11323 }
11324
11325 @Test
11326 public void testConstructor107() throws WrongSizeException {
11327 final Matrix ba = generateBa();
11328 final double biasX = ba.getElementAtIndex(0);
11329 final double biasY = ba.getElementAtIndex(1);
11330 final double biasZ = ba.getElementAtIndex(2);
11331
11332 final Matrix ma = generateMaCommonAxis();
11333 final double sx = ma.getElementAt(0, 0);
11334 final double sy = ma.getElementAt(1, 1);
11335 final double sz = ma.getElementAt(2, 2);
11336
11337 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11338 final double latitude = Math.toRadians(
11339 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11340 final double longitude = Math.toRadians(
11341 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11342 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11343 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11344 final NEDVelocity nedVelocity = new NEDVelocity();
11345 final ECEFPosition ecefPosition = new ECEFPosition();
11346 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11347 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11348 ecefPosition, ecefVelocity);
11349 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11350 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11351 final double gravityNorm = gravity.getNorm();
11352
11353 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11354 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11355 true, biasX, biasY, biasZ, sx, sy, sz,
11356 this);
11357
11358
11359 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11360 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11361 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11362 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11363 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11364 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11365 final Acceleration bx2 = new Acceleration(0.0,
11366 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11367 calibrator.getBiasXAsAcceleration(bx2);
11368 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11369 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11370 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11371 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11372 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11373 final Acceleration by2 = new Acceleration(0.0,
11374 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11375 calibrator.getBiasYAsAcceleration(by2);
11376 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11377 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11378 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11379 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11380 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11381 final Acceleration bz2 = new Acceleration(0.0,
11382 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11383 calibrator.getBiasZAsAcceleration(bz2);
11384 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11385 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11386 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11387 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11388 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11389 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11390 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11391 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11392 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11393 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11394 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11395 final double[] bias1 = calibrator.getBias();
11396 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11397 final double[] bias2 = new double[3];
11398 calibrator.getBias(bias2);
11399 assertArrayEquals(bias1, bias2, 0.0);
11400 final Matrix b1 = calibrator.getBiasAsMatrix();
11401 assertEquals(b1, ba);
11402 final Matrix b2 = new Matrix(3, 1);
11403 calibrator.getBiasAsMatrix(b2);
11404 assertEquals(b1, b2);
11405 final Matrix ma1 = calibrator.getInitialMa();
11406 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11407 final Matrix ma2 = new Matrix(3, 3);
11408 calibrator.getInitialMa(ma2);
11409 assertEquals(ma1, ma2);
11410 assertNull(calibrator.getMeasurements());
11411 assertTrue(calibrator.isCommonAxisUsed());
11412 assertSame(calibrator.getListener(), this);
11413 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11414 assertFalse(calibrator.isReady());
11415 assertFalse(calibrator.isRunning());
11416 assertNull(calibrator.getEstimatedMa());
11417 assertNull(calibrator.getEstimatedSx());
11418 assertNull(calibrator.getEstimatedSy());
11419 assertNull(calibrator.getEstimatedSz());
11420 assertNull(calibrator.getEstimatedMxy());
11421 assertNull(calibrator.getEstimatedMxz());
11422 assertNull(calibrator.getEstimatedMyx());
11423 assertNull(calibrator.getEstimatedMyz());
11424 assertNull(calibrator.getEstimatedMzx());
11425 assertNull(calibrator.getEstimatedMzy());
11426 assertNull(calibrator.getEstimatedCovariance());
11427 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11428 assertNotNull(calibrator.getGroundTruthGravityNorm());
11429 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11430 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11431 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11432 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11433 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11434 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11435 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11436
11437
11438 calibrator = null;
11439 try {
11440 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11441 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
11442 this);
11443 fail("IllegalArgumentException expected but not thrown");
11444 } catch (final IllegalArgumentException ignore) {
11445 }
11446 assertNull(calibrator);
11447 }
11448
11449 @Test
11450 public void testConstructor108() throws WrongSizeException {
11451 final Collection<StandardDeviationBodyKinematics> measurements =
11452 Collections.emptyList();
11453
11454 final Matrix ba = generateBa();
11455 final double biasX = ba.getElementAtIndex(0);
11456 final double biasY = ba.getElementAtIndex(1);
11457 final double biasZ = ba.getElementAtIndex(2);
11458
11459 final Matrix ma = generateMaCommonAxis();
11460 final double sx = ma.getElementAt(0, 0);
11461 final double sy = ma.getElementAt(1, 1);
11462 final double sz = ma.getElementAt(2, 2);
11463
11464 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11465 final double latitude = Math.toRadians(
11466 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11467 final double longitude = Math.toRadians(
11468 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11469 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11470 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11471 final NEDVelocity nedVelocity = new NEDVelocity();
11472 final ECEFPosition ecefPosition = new ECEFPosition();
11473 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11474 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11475 ecefPosition, ecefVelocity);
11476 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11477 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11478 final double gravityNorm = gravity.getNorm();
11479
11480 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11481 new KnownBiasAndGravityNormAccelerometerCalibrator(
11482 gravityNorm, measurements,
11483 true, biasX, biasY, biasZ,
11484 sx, sy, sz);
11485
11486
11487 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11488 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11489 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11490 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11491 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11492 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11493 final Acceleration bx2 = new Acceleration(0.0,
11494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11495 calibrator.getBiasXAsAcceleration(bx2);
11496 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11497 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11498 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11499 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11500 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11501 final Acceleration by2 = new Acceleration(0.0,
11502 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11503 calibrator.getBiasYAsAcceleration(by2);
11504 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11505 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11506 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11507 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11508 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11509 final Acceleration bz2 = new Acceleration(0.0,
11510 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11511 calibrator.getBiasZAsAcceleration(bz2);
11512 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11513 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11514 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11515 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11516 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11517 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11518 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11519 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11520 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11521 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11522 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11523 final double[] bias1 = calibrator.getBias();
11524 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11525 final double[] bias2 = new double[3];
11526 calibrator.getBias(bias2);
11527 assertArrayEquals(bias1, bias2, 0.0);
11528 final Matrix b1 = calibrator.getBiasAsMatrix();
11529 assertEquals(b1, ba);
11530 final Matrix b2 = new Matrix(3, 1);
11531 calibrator.getBiasAsMatrix(b2);
11532 assertEquals(b1, b2);
11533 final Matrix ma1 = calibrator.getInitialMa();
11534 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11535 final Matrix ma2 = new Matrix(3, 3);
11536 calibrator.getInitialMa(ma2);
11537 assertEquals(ma1, ma2);
11538 assertSame(calibrator.getMeasurements(), measurements);
11539 assertTrue(calibrator.isCommonAxisUsed());
11540 assertNull(calibrator.getListener());
11541 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11542 assertFalse(calibrator.isReady());
11543 assertFalse(calibrator.isRunning());
11544 assertNull(calibrator.getEstimatedMa());
11545 assertNull(calibrator.getEstimatedSx());
11546 assertNull(calibrator.getEstimatedSy());
11547 assertNull(calibrator.getEstimatedSz());
11548 assertNull(calibrator.getEstimatedMxy());
11549 assertNull(calibrator.getEstimatedMxz());
11550 assertNull(calibrator.getEstimatedMyx());
11551 assertNull(calibrator.getEstimatedMyz());
11552 assertNull(calibrator.getEstimatedMzx());
11553 assertNull(calibrator.getEstimatedMzy());
11554 assertNull(calibrator.getEstimatedCovariance());
11555 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11556 assertNotNull(calibrator.getGroundTruthGravityNorm());
11557 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11558 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11559 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11560 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11561 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11562 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11563 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11564
11565
11566 calibrator = null;
11567 try {
11568 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11569 -gravityNorm, measurements,
11570 true, biasX, biasY, biasZ,
11571 sx, sy, sz);
11572 fail("IllegalArgumentException expected but not thrown");
11573 } catch (final IllegalArgumentException ignore) {
11574 }
11575 assertNull(calibrator);
11576 }
11577
11578 @Test
11579 public void testConstructor109() throws WrongSizeException {
11580 final Collection<StandardDeviationBodyKinematics> measurements =
11581 Collections.emptyList();
11582
11583 final Matrix ba = generateBa();
11584 final double biasX = ba.getElementAtIndex(0);
11585 final double biasY = ba.getElementAtIndex(1);
11586 final double biasZ = ba.getElementAtIndex(2);
11587
11588 final Matrix ma = generateMaCommonAxis();
11589 final double sx = ma.getElementAt(0, 0);
11590 final double sy = ma.getElementAt(1, 1);
11591 final double sz = ma.getElementAt(2, 2);
11592
11593 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11594 final double latitude = Math.toRadians(
11595 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11596 final double longitude = Math.toRadians(
11597 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11598 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11599 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11600 final NEDVelocity nedVelocity = new NEDVelocity();
11601 final ECEFPosition ecefPosition = new ECEFPosition();
11602 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11603 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11604 ecefPosition, ecefVelocity);
11605 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11606 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11607 final double gravityNorm = gravity.getNorm();
11608
11609 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11610 new KnownBiasAndGravityNormAccelerometerCalibrator(
11611 gravityNorm, measurements,
11612 true, biasX, biasY, biasZ, sx, sy, sz,
11613 this);
11614
11615
11616 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11617 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11618 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11619 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11620 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11621 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11622 final Acceleration bx2 = new Acceleration(0.0,
11623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11624 calibrator.getBiasXAsAcceleration(bx2);
11625 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11626 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11627 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11628 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11629 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11630 final Acceleration by2 = new Acceleration(0.0,
11631 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11632 calibrator.getBiasYAsAcceleration(by2);
11633 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11634 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11635 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11636 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11637 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11638 final Acceleration bz2 = new Acceleration(0.0,
11639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11640 calibrator.getBiasZAsAcceleration(bz2);
11641 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11642 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11643 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11644 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11645 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11646 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11647 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11648 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11649 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11650 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11651 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11652 final double[] bias1 = calibrator.getBias();
11653 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11654 final double[] bias2 = new double[3];
11655 calibrator.getBias(bias2);
11656 assertArrayEquals(bias1, bias2, 0.0);
11657 final Matrix b1 = calibrator.getBiasAsMatrix();
11658 assertEquals(b1, ba);
11659 final Matrix b2 = new Matrix(3, 1);
11660 calibrator.getBiasAsMatrix(b2);
11661 assertEquals(b1, b2);
11662 final Matrix ma1 = calibrator.getInitialMa();
11663 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11664 final Matrix ma2 = new Matrix(3, 3);
11665 calibrator.getInitialMa(ma2);
11666 assertEquals(ma1, ma2);
11667 assertSame(calibrator.getMeasurements(), measurements);
11668 assertTrue(calibrator.isCommonAxisUsed());
11669 assertSame(calibrator.getListener(), this);
11670 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11671 assertFalse(calibrator.isReady());
11672 assertFalse(calibrator.isRunning());
11673 assertNull(calibrator.getEstimatedMa());
11674 assertNull(calibrator.getEstimatedSx());
11675 assertNull(calibrator.getEstimatedSy());
11676 assertNull(calibrator.getEstimatedSz());
11677 assertNull(calibrator.getEstimatedMxy());
11678 assertNull(calibrator.getEstimatedMxz());
11679 assertNull(calibrator.getEstimatedMyx());
11680 assertNull(calibrator.getEstimatedMyz());
11681 assertNull(calibrator.getEstimatedMzx());
11682 assertNull(calibrator.getEstimatedMzy());
11683 assertNull(calibrator.getEstimatedCovariance());
11684 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11685 assertNotNull(calibrator.getGroundTruthGravityNorm());
11686 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11687 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11688 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11689 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11690 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11691 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11692 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11693
11694
11695 calibrator = null;
11696 try {
11697 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11698 -gravityNorm, measurements,
11699 true, biasX, biasY, biasZ, sx, sy, sz,
11700 this);
11701 fail("IllegalArgumentException expected but not thrown");
11702 } catch (final IllegalArgumentException ignore) {
11703 }
11704 assertNull(calibrator);
11705 }
11706
11707 @Test
11708 public void testConstructor110() throws WrongSizeException {
11709 final Matrix ba = generateBa();
11710 final double biasX = ba.getElementAtIndex(0);
11711 final double biasY = ba.getElementAtIndex(1);
11712 final double biasZ = ba.getElementAtIndex(2);
11713
11714 final Acceleration bx = new Acceleration(biasX,
11715 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11716 final Acceleration by = new Acceleration(biasY,
11717 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11718 final Acceleration bz = new Acceleration(biasZ,
11719 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11720
11721 final Matrix ma = generateMaCommonAxis();
11722 final double sx = ma.getElementAt(0, 0);
11723 final double sy = ma.getElementAt(1, 1);
11724 final double sz = ma.getElementAt(2, 2);
11725
11726 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11727 final double latitude = Math.toRadians(
11728 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11729 final double longitude = Math.toRadians(
11730 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11731 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11732 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11733 final NEDVelocity nedVelocity = new NEDVelocity();
11734 final ECEFPosition ecefPosition = new ECEFPosition();
11735 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11736 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11737 ecefPosition, ecefVelocity);
11738 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11739 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11740 final double gravityNorm = gravity.getNorm();
11741
11742 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11743 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11744 bx, by, bz, sx, sy, sz);
11745
11746
11747 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11748 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11749 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11750 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11751 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11752 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11753 final Acceleration bx2 = new Acceleration(0.0,
11754 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11755 calibrator.getBiasXAsAcceleration(bx2);
11756 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11757 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11758 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11759 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11760 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11761 final Acceleration by2 = new Acceleration(0.0,
11762 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11763 calibrator.getBiasYAsAcceleration(by2);
11764 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11765 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11766 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11767 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11768 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11769 final Acceleration bz2 = new Acceleration(0.0,
11770 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11771 calibrator.getBiasZAsAcceleration(bz2);
11772 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11773 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11774 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11775 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11776 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11777 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11778 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11779 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11780 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11781 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11782 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11783 final double[] bias1 = calibrator.getBias();
11784 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11785 final double[] bias2 = new double[3];
11786 calibrator.getBias(bias2);
11787 assertArrayEquals(bias1, bias2, 0.0);
11788 final Matrix b1 = calibrator.getBiasAsMatrix();
11789 assertEquals(b1, ba);
11790 final Matrix b2 = new Matrix(3, 1);
11791 calibrator.getBiasAsMatrix(b2);
11792 assertEquals(b1, b2);
11793 final Matrix ma1 = calibrator.getInitialMa();
11794 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11795 final Matrix ma2 = new Matrix(3, 3);
11796 calibrator.getInitialMa(ma2);
11797 assertEquals(ma1, ma2);
11798 assertNull(calibrator.getMeasurements());
11799 assertFalse(calibrator.isCommonAxisUsed());
11800 assertNull(calibrator.getListener());
11801 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11802 assertFalse(calibrator.isReady());
11803 assertFalse(calibrator.isRunning());
11804 assertNull(calibrator.getEstimatedMa());
11805 assertNull(calibrator.getEstimatedSx());
11806 assertNull(calibrator.getEstimatedSy());
11807 assertNull(calibrator.getEstimatedSz());
11808 assertNull(calibrator.getEstimatedMxy());
11809 assertNull(calibrator.getEstimatedMxz());
11810 assertNull(calibrator.getEstimatedMyx());
11811 assertNull(calibrator.getEstimatedMyz());
11812 assertNull(calibrator.getEstimatedMzx());
11813 assertNull(calibrator.getEstimatedMzy());
11814 assertNull(calibrator.getEstimatedCovariance());
11815 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11816 assertNotNull(calibrator.getGroundTruthGravityNorm());
11817 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11818 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11819 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11820 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11821 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11822 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11823 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11824
11825
11826 calibrator = null;
11827 try {
11828 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11829 -gravityNorm, bx, by, bz, sx, sy, sz);
11830 fail("IllegalArgumentException expected but not thrown");
11831 } catch (final IllegalArgumentException ignore) {
11832 }
11833 assertNull(calibrator);
11834 }
11835
11836 @Test
11837 public void testConstructor111() throws WrongSizeException {
11838 final Matrix ba = generateBa();
11839 final double biasX = ba.getElementAtIndex(0);
11840 final double biasY = ba.getElementAtIndex(1);
11841 final double biasZ = ba.getElementAtIndex(2);
11842
11843 final Acceleration bx = new Acceleration(biasX,
11844 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11845 final Acceleration by = new Acceleration(biasY,
11846 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11847 final Acceleration bz = new Acceleration(biasZ,
11848 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11849
11850 final Matrix ma = generateMaCommonAxis();
11851 final double sx = ma.getElementAt(0, 0);
11852 final double sy = ma.getElementAt(1, 1);
11853 final double sz = ma.getElementAt(2, 2);
11854
11855 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11856 final double latitude = Math.toRadians(
11857 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11858 final double longitude = Math.toRadians(
11859 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11860 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11861 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11862 final NEDVelocity nedVelocity = new NEDVelocity();
11863 final ECEFPosition ecefPosition = new ECEFPosition();
11864 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11865 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11866 ecefPosition, ecefVelocity);
11867 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11868 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11869 final double gravityNorm = gravity.getNorm();
11870
11871 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11872 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11873 bx, by, bz, sx, sy, sz, this);
11874
11875
11876 assertEquals(calibrator.getBiasX(), biasX, 0.0);
11877 assertEquals(calibrator.getBiasY(), biasY, 0.0);
11878 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11879 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11880 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11881 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11882 final Acceleration bx2 = new Acceleration(0.0,
11883 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11884 calibrator.getBiasXAsAcceleration(bx2);
11885 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11886 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11887 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11888 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11889 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11890 final Acceleration by2 = new Acceleration(0.0,
11891 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11892 calibrator.getBiasYAsAcceleration(by2);
11893 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11894 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11895 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11896 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11897 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11898 final Acceleration bz2 = new Acceleration(0.0,
11899 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11900 calibrator.getBiasZAsAcceleration(bz2);
11901 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11902 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11903 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11904 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11905 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11906 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11907 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11908 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11909 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11910 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11911 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11912 final double[] bias1 = calibrator.getBias();
11913 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11914 final double[] bias2 = new double[3];
11915 calibrator.getBias(bias2);
11916 assertArrayEquals(bias1, bias2, 0.0);
11917 final Matrix b1 = calibrator.getBiasAsMatrix();
11918 assertEquals(b1, ba);
11919 final Matrix b2 = new Matrix(3, 1);
11920 calibrator.getBiasAsMatrix(b2);
11921 assertEquals(b1, b2);
11922 final Matrix ma1 = calibrator.getInitialMa();
11923 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11924 final Matrix ma2 = new Matrix(3, 3);
11925 calibrator.getInitialMa(ma2);
11926 assertEquals(ma1, ma2);
11927 assertNull(calibrator.getMeasurements());
11928 assertFalse(calibrator.isCommonAxisUsed());
11929 assertSame(calibrator.getListener(), this);
11930 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11931 assertFalse(calibrator.isReady());
11932 assertFalse(calibrator.isRunning());
11933 assertNull(calibrator.getEstimatedMa());
11934 assertNull(calibrator.getEstimatedSx());
11935 assertNull(calibrator.getEstimatedSy());
11936 assertNull(calibrator.getEstimatedSz());
11937 assertNull(calibrator.getEstimatedMxy());
11938 assertNull(calibrator.getEstimatedMxz());
11939 assertNull(calibrator.getEstimatedMyx());
11940 assertNull(calibrator.getEstimatedMyz());
11941 assertNull(calibrator.getEstimatedMzx());
11942 assertNull(calibrator.getEstimatedMzy());
11943 assertNull(calibrator.getEstimatedCovariance());
11944 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11945 assertNotNull(calibrator.getGroundTruthGravityNorm());
11946 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11947 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11948 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11949 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11950 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11951 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11952 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11953
11954
11955 calibrator = null;
11956 try {
11957 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11958 -gravityNorm, bx, by, bz, sx, sy, sz, this);
11959 fail("IllegalArgumentException expected but not thrown");
11960 } catch (final IllegalArgumentException ignore) {
11961 }
11962 assertNull(calibrator);
11963 }
11964
11965 @Test
11966 public void testConstructor112() throws WrongSizeException {
11967 final Collection<StandardDeviationBodyKinematics> measurements =
11968 Collections.emptyList();
11969
11970 final Matrix ba = generateBa();
11971 final double biasX = ba.getElementAtIndex(0);
11972 final double biasY = ba.getElementAtIndex(1);
11973 final double biasZ = ba.getElementAtIndex(2);
11974
11975 final Acceleration bx = new Acceleration(biasX,
11976 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11977 final Acceleration by = new Acceleration(biasY,
11978 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11979 final Acceleration bz = new Acceleration(biasZ,
11980 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11981
11982 final Matrix ma = generateMaCommonAxis();
11983 final double sx = ma.getElementAt(0, 0);
11984 final double sy = ma.getElementAt(1, 1);
11985 final double sz = ma.getElementAt(2, 2);
11986
11987 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11988 final double latitude = Math.toRadians(
11989 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11990 final double longitude = Math.toRadians(
11991 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11992 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11993 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11994 final NEDVelocity nedVelocity = new NEDVelocity();
11995 final ECEFPosition ecefPosition = new ECEFPosition();
11996 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11997 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11998 ecefPosition, ecefVelocity);
11999 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12000 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12001 final double gravityNorm = gravity.getNorm();
12002
12003 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12004 new KnownBiasAndGravityNormAccelerometerCalibrator(
12005 gravityNorm, measurements,
12006 bx, by, bz, sx, sy, sz);
12007
12008
12009 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12010 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12011 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12012 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12013 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12014 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12015 final Acceleration bx2 = new Acceleration(0.0,
12016 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12017 calibrator.getBiasXAsAcceleration(bx2);
12018 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12019 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12020 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12021 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12022 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12023 final Acceleration by2 = new Acceleration(0.0,
12024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12025 calibrator.getBiasYAsAcceleration(by2);
12026 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12027 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12028 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12029 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12030 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12031 final Acceleration bz2 = new Acceleration(0.0,
12032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12033 calibrator.getBiasZAsAcceleration(bz2);
12034 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12035 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12036 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12037 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12038 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12039 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12040 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12041 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12042 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12043 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12044 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12045 final double[] bias1 = calibrator.getBias();
12046 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12047 final double[] bias2 = new double[3];
12048 calibrator.getBias(bias2);
12049 assertArrayEquals(bias1, bias2, 0.0);
12050 final Matrix b1 = calibrator.getBiasAsMatrix();
12051 assertEquals(b1, ba);
12052 final Matrix b2 = new Matrix(3, 1);
12053 calibrator.getBiasAsMatrix(b2);
12054 assertEquals(b1, b2);
12055 final Matrix ma1 = calibrator.getInitialMa();
12056 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12057 final Matrix ma2 = new Matrix(3, 3);
12058 calibrator.getInitialMa(ma2);
12059 assertEquals(ma1, ma2);
12060 assertSame(calibrator.getMeasurements(), measurements);
12061 assertFalse(calibrator.isCommonAxisUsed());
12062 assertNull(calibrator.getListener());
12063 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12064 assertFalse(calibrator.isReady());
12065 assertFalse(calibrator.isRunning());
12066 assertNull(calibrator.getEstimatedMa());
12067 assertNull(calibrator.getEstimatedSx());
12068 assertNull(calibrator.getEstimatedSy());
12069 assertNull(calibrator.getEstimatedSz());
12070 assertNull(calibrator.getEstimatedMxy());
12071 assertNull(calibrator.getEstimatedMxz());
12072 assertNull(calibrator.getEstimatedMyx());
12073 assertNull(calibrator.getEstimatedMyz());
12074 assertNull(calibrator.getEstimatedMzx());
12075 assertNull(calibrator.getEstimatedMzy());
12076 assertNull(calibrator.getEstimatedCovariance());
12077 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12078 assertNotNull(calibrator.getGroundTruthGravityNorm());
12079 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12080 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12081 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12082 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12083 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12084 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12085 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12086
12087
12088 calibrator = null;
12089 try {
12090 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12091 -gravityNorm, measurements,
12092 bx, by, bz, sx, sy, sz);
12093 fail("IllegalArgumentException expected but not thrown");
12094 } catch (final IllegalArgumentException ignore) {
12095 }
12096 assertNull(calibrator);
12097 }
12098
12099 @Test
12100 public void testConstructor113() throws WrongSizeException {
12101 final Collection<StandardDeviationBodyKinematics> measurements =
12102 Collections.emptyList();
12103
12104 final Matrix ba = generateBa();
12105 final double biasX = ba.getElementAtIndex(0);
12106 final double biasY = ba.getElementAtIndex(1);
12107 final double biasZ = ba.getElementAtIndex(2);
12108
12109 final Acceleration bx = new Acceleration(biasX,
12110 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12111 final Acceleration by = new Acceleration(biasY,
12112 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12113 final Acceleration bz = new Acceleration(biasZ,
12114 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12115
12116 final Matrix ma = generateMaCommonAxis();
12117 final double sx = ma.getElementAt(0, 0);
12118 final double sy = ma.getElementAt(1, 1);
12119 final double sz = ma.getElementAt(2, 2);
12120
12121 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12122 final double latitude = Math.toRadians(
12123 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12124 final double longitude = Math.toRadians(
12125 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12126 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12127 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12128 final NEDVelocity nedVelocity = new NEDVelocity();
12129 final ECEFPosition ecefPosition = new ECEFPosition();
12130 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12131 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12132 ecefPosition, ecefVelocity);
12133 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12134 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12135 final double gravityNorm = gravity.getNorm();
12136
12137 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12138 new KnownBiasAndGravityNormAccelerometerCalibrator(
12139 gravityNorm, measurements,
12140 bx, by, bz, sx, sy, sz, this);
12141
12142
12143 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12144 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12145 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12146 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12147 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12148 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12149 final Acceleration bx2 = new Acceleration(0.0,
12150 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12151 calibrator.getBiasXAsAcceleration(bx2);
12152 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12153 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12154 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12155 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12156 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12157 final Acceleration by2 = new Acceleration(0.0,
12158 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12159 calibrator.getBiasYAsAcceleration(by2);
12160 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12161 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12162 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12163 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12164 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12165 final Acceleration bz2 = new Acceleration(0.0,
12166 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12167 calibrator.getBiasZAsAcceleration(bz2);
12168 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12169 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12170 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12171 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12172 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12173 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12174 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12175 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12176 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12177 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12178 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12179 final double[] bias1 = calibrator.getBias();
12180 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12181 final double[] bias2 = new double[3];
12182 calibrator.getBias(bias2);
12183 assertArrayEquals(bias1, bias2, 0.0);
12184 final Matrix b1 = calibrator.getBiasAsMatrix();
12185 assertEquals(b1, ba);
12186 final Matrix b2 = new Matrix(3, 1);
12187 calibrator.getBiasAsMatrix(b2);
12188 assertEquals(b1, b2);
12189 final Matrix ma1 = calibrator.getInitialMa();
12190 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12191 final Matrix ma2 = new Matrix(3, 3);
12192 calibrator.getInitialMa(ma2);
12193 assertEquals(ma1, ma2);
12194 assertSame(calibrator.getMeasurements(), measurements);
12195 assertFalse(calibrator.isCommonAxisUsed());
12196 assertSame(calibrator.getListener(), this);
12197 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12198 assertFalse(calibrator.isReady());
12199 assertFalse(calibrator.isRunning());
12200 assertNull(calibrator.getEstimatedMa());
12201 assertNull(calibrator.getEstimatedSx());
12202 assertNull(calibrator.getEstimatedSy());
12203 assertNull(calibrator.getEstimatedSz());
12204 assertNull(calibrator.getEstimatedMxy());
12205 assertNull(calibrator.getEstimatedMxz());
12206 assertNull(calibrator.getEstimatedMyx());
12207 assertNull(calibrator.getEstimatedMyz());
12208 assertNull(calibrator.getEstimatedMzx());
12209 assertNull(calibrator.getEstimatedMzy());
12210 assertNull(calibrator.getEstimatedCovariance());
12211 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12212 assertNotNull(calibrator.getGroundTruthGravityNorm());
12213 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12214 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12215 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12216 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12217 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12218 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12219 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12220
12221
12222 calibrator = null;
12223 try {
12224 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12225 -gravityNorm, measurements,
12226 bx, by, bz, sx, sy, sz, this);
12227 fail("IllegalArgumentException expected but not thrown");
12228 } catch (final IllegalArgumentException ignore) {
12229 }
12230 assertNull(calibrator);
12231 }
12232
12233 @Test
12234 public void testConstructor114() throws WrongSizeException {
12235 final Matrix ba = generateBa();
12236 final double biasX = ba.getElementAtIndex(0);
12237 final double biasY = ba.getElementAtIndex(1);
12238 final double biasZ = ba.getElementAtIndex(2);
12239
12240 final Acceleration bx = new Acceleration(biasX,
12241 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12242 final Acceleration by = new Acceleration(biasY,
12243 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12244 final Acceleration bz = new Acceleration(biasZ,
12245 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12246
12247 final Matrix ma = generateMaCommonAxis();
12248 final double sx = ma.getElementAt(0, 0);
12249 final double sy = ma.getElementAt(1, 1);
12250 final double sz = ma.getElementAt(2, 2);
12251
12252 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12253 final double latitude = Math.toRadians(
12254 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12255 final double longitude = Math.toRadians(
12256 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12257 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12258 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12259 final NEDVelocity nedVelocity = new NEDVelocity();
12260 final ECEFPosition ecefPosition = new ECEFPosition();
12261 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12262 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12263 ecefPosition, ecefVelocity);
12264 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12265 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12266 final double gravityNorm = gravity.getNorm();
12267
12268 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12269 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12270 true, bx, by, bz, sx, sy, sz);
12271
12272
12273 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12274 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12275 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12276 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12277 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12278 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12279 final Acceleration bx2 = new Acceleration(0.0,
12280 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12281 calibrator.getBiasXAsAcceleration(bx2);
12282 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12283 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12284 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12285 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12286 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12287 final Acceleration by2 = new Acceleration(0.0,
12288 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12289 calibrator.getBiasYAsAcceleration(by2);
12290 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12291 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12292 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12293 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12294 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12295 final Acceleration bz2 = new Acceleration(0.0,
12296 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12297 calibrator.getBiasZAsAcceleration(bz2);
12298 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12299 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12300 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12301 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12302 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12303 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12304 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12305 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12306 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12307 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12308 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12309 final double[] bias1 = calibrator.getBias();
12310 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12311 final double[] bias2 = new double[3];
12312 calibrator.getBias(bias2);
12313 assertArrayEquals(bias1, bias2, 0.0);
12314 final Matrix b1 = calibrator.getBiasAsMatrix();
12315 assertEquals(b1, ba);
12316 final Matrix b2 = new Matrix(3, 1);
12317 calibrator.getBiasAsMatrix(b2);
12318 assertEquals(b1, b2);
12319 final Matrix ma1 = calibrator.getInitialMa();
12320 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12321 final Matrix ma2 = new Matrix(3, 3);
12322 calibrator.getInitialMa(ma2);
12323 assertEquals(ma1, ma2);
12324 assertNull(calibrator.getMeasurements());
12325 assertTrue(calibrator.isCommonAxisUsed());
12326 assertNull(calibrator.getListener());
12327 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12328 assertFalse(calibrator.isReady());
12329 assertFalse(calibrator.isRunning());
12330 assertNull(calibrator.getEstimatedMa());
12331 assertNull(calibrator.getEstimatedSx());
12332 assertNull(calibrator.getEstimatedSy());
12333 assertNull(calibrator.getEstimatedSz());
12334 assertNull(calibrator.getEstimatedMxy());
12335 assertNull(calibrator.getEstimatedMxz());
12336 assertNull(calibrator.getEstimatedMyx());
12337 assertNull(calibrator.getEstimatedMyz());
12338 assertNull(calibrator.getEstimatedMzx());
12339 assertNull(calibrator.getEstimatedMzy());
12340 assertNull(calibrator.getEstimatedCovariance());
12341 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12342 assertNotNull(calibrator.getGroundTruthGravityNorm());
12343 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12344 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12345 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12346 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12347 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12348 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12349 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12350
12351
12352 calibrator = null;
12353 try {
12354 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12355 -gravityNorm, true, bx, by, bz,
12356 sx, sy, sz);
12357 fail("IllegalArgumentException expected but not thrown");
12358 } catch (final IllegalArgumentException ignore) {
12359 }
12360 assertNull(calibrator);
12361 }
12362
12363 @Test
12364 public void testConstructor115() throws WrongSizeException {
12365 final Matrix ba = generateBa();
12366 final double biasX = ba.getElementAtIndex(0);
12367 final double biasY = ba.getElementAtIndex(1);
12368 final double biasZ = ba.getElementAtIndex(2);
12369
12370 final Acceleration bx = new Acceleration(biasX,
12371 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12372 final Acceleration by = new Acceleration(biasY,
12373 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12374 final Acceleration bz = new Acceleration(biasZ,
12375 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12376
12377 final Matrix ma = generateMaCommonAxis();
12378 final double sx = ma.getElementAt(0, 0);
12379 final double sy = ma.getElementAt(1, 1);
12380 final double sz = ma.getElementAt(2, 2);
12381
12382 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12383 final double latitude = Math.toRadians(
12384 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12385 final double longitude = Math.toRadians(
12386 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12387 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12388 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12389 final NEDVelocity nedVelocity = new NEDVelocity();
12390 final ECEFPosition ecefPosition = new ECEFPosition();
12391 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12392 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12393 ecefPosition, ecefVelocity);
12394 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12395 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12396 final double gravityNorm = gravity.getNorm();
12397
12398 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12399 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12400 true, bx, by, bz, sx, sy, sz,
12401 this);
12402
12403
12404 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12405 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12406 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12407 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12408 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12409 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12410 final Acceleration bx2 = new Acceleration(0.0,
12411 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12412 calibrator.getBiasXAsAcceleration(bx2);
12413 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12414 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12415 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12416 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12417 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12418 final Acceleration by2 = new Acceleration(0.0,
12419 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12420 calibrator.getBiasYAsAcceleration(by2);
12421 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12422 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12423 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12424 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12425 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12426 final Acceleration bz2 = new Acceleration(0.0,
12427 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12428 calibrator.getBiasZAsAcceleration(bz2);
12429 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12430 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12431 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12432 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12433 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12434 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12435 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12436 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12437 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12438 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12439 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12440 final double[] bias1 = calibrator.getBias();
12441 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12442 final double[] bias2 = new double[3];
12443 calibrator.getBias(bias2);
12444 assertArrayEquals(bias1, bias2, 0.0);
12445 final Matrix b1 = calibrator.getBiasAsMatrix();
12446 assertEquals(b1, ba);
12447 final Matrix b2 = new Matrix(3, 1);
12448 calibrator.getBiasAsMatrix(b2);
12449 assertEquals(b1, b2);
12450 final Matrix ma1 = calibrator.getInitialMa();
12451 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12452 final Matrix ma2 = new Matrix(3, 3);
12453 calibrator.getInitialMa(ma2);
12454 assertEquals(ma1, ma2);
12455 assertNull(calibrator.getMeasurements());
12456 assertTrue(calibrator.isCommonAxisUsed());
12457 assertSame(calibrator.getListener(), this);
12458 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12459 assertFalse(calibrator.isReady());
12460 assertFalse(calibrator.isRunning());
12461 assertNull(calibrator.getEstimatedMa());
12462 assertNull(calibrator.getEstimatedSx());
12463 assertNull(calibrator.getEstimatedSy());
12464 assertNull(calibrator.getEstimatedSz());
12465 assertNull(calibrator.getEstimatedMxy());
12466 assertNull(calibrator.getEstimatedMxz());
12467 assertNull(calibrator.getEstimatedMyx());
12468 assertNull(calibrator.getEstimatedMyz());
12469 assertNull(calibrator.getEstimatedMzx());
12470 assertNull(calibrator.getEstimatedMzy());
12471 assertNull(calibrator.getEstimatedCovariance());
12472 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12473 assertNotNull(calibrator.getGroundTruthGravityNorm());
12474 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12475 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12476 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12477 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12478 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12479 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12480 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12481
12482
12483 calibrator = null;
12484 try {
12485 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12486 -gravityNorm, true, bx, by, bz, sx, sy, sz,
12487 this);
12488 fail("IllegalArgumentException expected but not thrown");
12489 } catch (final IllegalArgumentException ignore) {
12490 }
12491 assertNull(calibrator);
12492 }
12493
12494 @Test
12495 public void testConstructor116() throws WrongSizeException {
12496 final Collection<StandardDeviationBodyKinematics> measurements =
12497 Collections.emptyList();
12498
12499 final Matrix ba = generateBa();
12500 final double biasX = ba.getElementAtIndex(0);
12501 final double biasY = ba.getElementAtIndex(1);
12502 final double biasZ = ba.getElementAtIndex(2);
12503
12504 final Acceleration bx = new Acceleration(biasX,
12505 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12506 final Acceleration by = new Acceleration(biasY,
12507 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12508 final Acceleration bz = new Acceleration(biasZ,
12509 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12510
12511 final Matrix ma = generateMaCommonAxis();
12512 final double sx = ma.getElementAt(0, 0);
12513 final double sy = ma.getElementAt(1, 1);
12514 final double sz = ma.getElementAt(2, 2);
12515
12516 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12517 final double latitude = Math.toRadians(
12518 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12519 final double longitude = Math.toRadians(
12520 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12521 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12522 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12523 final NEDVelocity nedVelocity = new NEDVelocity();
12524 final ECEFPosition ecefPosition = new ECEFPosition();
12525 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12526 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12527 ecefPosition, ecefVelocity);
12528 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12529 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12530 final double gravityNorm = gravity.getNorm();
12531
12532 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12533 new KnownBiasAndGravityNormAccelerometerCalibrator(
12534 gravityNorm, measurements,
12535 true, bx, by, bz, sx, sy, sz);
12536
12537
12538 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12539 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12540 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12541 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12542 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12543 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12544 final Acceleration bx2 = new Acceleration(0.0,
12545 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12546 calibrator.getBiasXAsAcceleration(bx2);
12547 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12548 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12549 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12550 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12551 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12552 final Acceleration by2 = new Acceleration(0.0,
12553 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12554 calibrator.getBiasYAsAcceleration(by2);
12555 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12556 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12557 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12558 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12559 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12560 final Acceleration bz2 = new Acceleration(0.0,
12561 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12562 calibrator.getBiasZAsAcceleration(bz2);
12563 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12564 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12565 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12566 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12567 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12568 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12569 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12570 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12571 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12572 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12573 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12574 final double[] bias1 = calibrator.getBias();
12575 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12576 final double[] bias2 = new double[3];
12577 calibrator.getBias(bias2);
12578 assertArrayEquals(bias1, bias2, 0.0);
12579 final Matrix b1 = calibrator.getBiasAsMatrix();
12580 assertEquals(b1, ba);
12581 final Matrix b2 = new Matrix(3, 1);
12582 calibrator.getBiasAsMatrix(b2);
12583 assertEquals(b1, b2);
12584 final Matrix ma1 = calibrator.getInitialMa();
12585 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12586 final Matrix ma2 = new Matrix(3, 3);
12587 calibrator.getInitialMa(ma2);
12588 assertEquals(ma1, ma2);
12589 assertSame(calibrator.getMeasurements(), measurements);
12590 assertTrue(calibrator.isCommonAxisUsed());
12591 assertNull(calibrator.getListener());
12592 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12593 assertFalse(calibrator.isReady());
12594 assertFalse(calibrator.isRunning());
12595 assertNull(calibrator.getEstimatedMa());
12596 assertNull(calibrator.getEstimatedSx());
12597 assertNull(calibrator.getEstimatedSy());
12598 assertNull(calibrator.getEstimatedSz());
12599 assertNull(calibrator.getEstimatedMxy());
12600 assertNull(calibrator.getEstimatedMxz());
12601 assertNull(calibrator.getEstimatedMyx());
12602 assertNull(calibrator.getEstimatedMyz());
12603 assertNull(calibrator.getEstimatedMzx());
12604 assertNull(calibrator.getEstimatedMzy());
12605 assertNull(calibrator.getEstimatedCovariance());
12606 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12607 assertNotNull(calibrator.getGroundTruthGravityNorm());
12608 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12609 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12610 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12611 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12612 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12613 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12614 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12615
12616
12617 calibrator = null;
12618 try {
12619 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12620 -gravityNorm, measurements,
12621 true, bx, by, bz, sx, sy, sz);
12622 fail("IllegalArgumentException expected but not thrown");
12623 } catch (final IllegalArgumentException ignore) {
12624 }
12625 assertNull(calibrator);
12626 }
12627
12628 @Test
12629 public void testConstructor117() throws WrongSizeException {
12630 final Collection<StandardDeviationBodyKinematics> measurements =
12631 Collections.emptyList();
12632
12633 final Matrix ba = generateBa();
12634 final double biasX = ba.getElementAtIndex(0);
12635 final double biasY = ba.getElementAtIndex(1);
12636 final double biasZ = ba.getElementAtIndex(2);
12637
12638 final Acceleration bx = new Acceleration(biasX,
12639 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12640 final Acceleration by = new Acceleration(biasY,
12641 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12642 final Acceleration bz = new Acceleration(biasZ,
12643 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12644
12645 final Matrix ma = generateMaCommonAxis();
12646 final double sx = ma.getElementAt(0, 0);
12647 final double sy = ma.getElementAt(1, 1);
12648 final double sz = ma.getElementAt(2, 2);
12649
12650 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12651 final double latitude = Math.toRadians(
12652 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12653 final double longitude = Math.toRadians(
12654 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12655 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12656 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12657 final NEDVelocity nedVelocity = new NEDVelocity();
12658 final ECEFPosition ecefPosition = new ECEFPosition();
12659 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12660 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12661 ecefPosition, ecefVelocity);
12662 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12663 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12664 final double gravityNorm = gravity.getNorm();
12665
12666 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12667 new KnownBiasAndGravityNormAccelerometerCalibrator(
12668 gravityNorm, measurements,
12669 true, bx, by, bz, sx, sy, sz, this);
12670
12671
12672 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12673 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12674 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12675 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12676 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12677 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12678 final Acceleration bx2 = new Acceleration(0.0,
12679 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12680 calibrator.getBiasXAsAcceleration(bx2);
12681 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12682 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12683 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12684 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12685 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12686 final Acceleration by2 = new Acceleration(0.0,
12687 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12688 calibrator.getBiasYAsAcceleration(by2);
12689 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12690 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12691 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12692 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12693 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12694 final Acceleration bz2 = new Acceleration(0.0,
12695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12696 calibrator.getBiasZAsAcceleration(bz2);
12697 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12698 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12699 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12700 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12701 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12702 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12703 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12704 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12705 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12706 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12707 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12708 final double[] bias1 = calibrator.getBias();
12709 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12710 final double[] bias2 = new double[3];
12711 calibrator.getBias(bias2);
12712 assertArrayEquals(bias1, bias2, 0.0);
12713 final Matrix b1 = calibrator.getBiasAsMatrix();
12714 assertEquals(b1, ba);
12715 final Matrix b2 = new Matrix(3, 1);
12716 calibrator.getBiasAsMatrix(b2);
12717 assertEquals(b1, b2);
12718 final Matrix ma1 = calibrator.getInitialMa();
12719 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12720 final Matrix ma2 = new Matrix(3, 3);
12721 calibrator.getInitialMa(ma2);
12722 assertEquals(ma1, ma2);
12723 assertSame(calibrator.getMeasurements(), measurements);
12724 assertTrue(calibrator.isCommonAxisUsed());
12725 assertSame(calibrator.getListener(), this);
12726 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12727 assertFalse(calibrator.isReady());
12728 assertFalse(calibrator.isRunning());
12729 assertNull(calibrator.getEstimatedMa());
12730 assertNull(calibrator.getEstimatedSx());
12731 assertNull(calibrator.getEstimatedSy());
12732 assertNull(calibrator.getEstimatedSz());
12733 assertNull(calibrator.getEstimatedMxy());
12734 assertNull(calibrator.getEstimatedMxz());
12735 assertNull(calibrator.getEstimatedMyx());
12736 assertNull(calibrator.getEstimatedMyz());
12737 assertNull(calibrator.getEstimatedMzx());
12738 assertNull(calibrator.getEstimatedMzy());
12739 assertNull(calibrator.getEstimatedCovariance());
12740 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12741 assertNotNull(calibrator.getGroundTruthGravityNorm());
12742 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12743 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12744 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12745 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12746 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12747 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12748 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12749
12750
12751 calibrator = null;
12752 try {
12753 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12754 -gravityNorm, measurements,
12755 true, bx, by, bz, sx, sy, sz, this);
12756 fail("IllegalArgumentException expected but not thrown");
12757 } catch (final IllegalArgumentException ignore) {
12758 }
12759 assertNull(calibrator);
12760 }
12761
12762 @Test
12763 public void testConstructor118() throws WrongSizeException {
12764 final Matrix ba = generateBa();
12765 final double biasX = ba.getElementAtIndex(0);
12766 final double biasY = ba.getElementAtIndex(1);
12767 final double biasZ = ba.getElementAtIndex(2);
12768
12769 final Matrix ma = generateMaCommonAxis();
12770 final double sx = ma.getElementAt(0, 0);
12771 final double sy = ma.getElementAt(1, 1);
12772 final double sz = ma.getElementAt(2, 2);
12773 final double mxy = ma.getElementAt(0, 1);
12774 final double mxz = ma.getElementAt(0, 2);
12775 final double myx = ma.getElementAt(1, 0);
12776 final double myz = ma.getElementAt(1, 2);
12777 final double mzx = ma.getElementAt(2, 0);
12778 final double mzy = ma.getElementAt(2, 1);
12779
12780 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12781 final double latitude = Math.toRadians(
12782 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12783 final double longitude = Math.toRadians(
12784 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12785 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12786 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12787 final NEDVelocity nedVelocity = new NEDVelocity();
12788 final ECEFPosition ecefPosition = new ECEFPosition();
12789 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12790 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12791 ecefPosition, ecefVelocity);
12792 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12793 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12794 final double gravityNorm = gravity.getNorm();
12795
12796 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12797 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12798 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
12799 myx, myz, mzx, mzy);
12800
12801
12802 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12803 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12804 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12805 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12806 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12807 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12808 final Acceleration bx2 = new Acceleration(0.0,
12809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12810 calibrator.getBiasXAsAcceleration(bx2);
12811 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12812 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12813 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12814 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12815 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12816 final Acceleration by2 = new Acceleration(0.0,
12817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12818 calibrator.getBiasYAsAcceleration(by2);
12819 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12820 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12821 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12822 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12823 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12824 final Acceleration bz2 = new Acceleration(0.0,
12825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12826 calibrator.getBiasZAsAcceleration(bz2);
12827 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12828 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12829 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12830 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12831 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12832 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
12833 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
12834 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
12835 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
12836 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
12837 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
12838 final double[] bias1 = calibrator.getBias();
12839 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12840 final double[] bias2 = new double[3];
12841 calibrator.getBias(bias2);
12842 assertArrayEquals(bias1, bias2, 0.0);
12843 final Matrix b1 = calibrator.getBiasAsMatrix();
12844 assertEquals(b1, ba);
12845 final Matrix b2 = new Matrix(3, 1);
12846 calibrator.getBiasAsMatrix(b2);
12847 assertEquals(b1, b2);
12848 final Matrix ma1 = new Matrix(3, 3);
12849 ma1.setSubmatrix(0, 0,
12850 2, 2,
12851 new double[]{sx, myx, mzx,
12852 mxy, sy, mzy,
12853 mxz, myz, sz});
12854 assertEquals(calibrator.getInitialMa(), ma1);
12855 final Matrix ma2 = new Matrix(3, 3);
12856 calibrator.getInitialMa(ma2);
12857 assertEquals(ma1, ma2);
12858 assertNull(calibrator.getMeasurements());
12859 assertFalse(calibrator.isCommonAxisUsed());
12860 assertNull(calibrator.getListener());
12861 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12862 assertFalse(calibrator.isReady());
12863 assertFalse(calibrator.isRunning());
12864 assertNull(calibrator.getEstimatedMa());
12865 assertNull(calibrator.getEstimatedSx());
12866 assertNull(calibrator.getEstimatedSy());
12867 assertNull(calibrator.getEstimatedSz());
12868 assertNull(calibrator.getEstimatedMxy());
12869 assertNull(calibrator.getEstimatedMxz());
12870 assertNull(calibrator.getEstimatedMyx());
12871 assertNull(calibrator.getEstimatedMyz());
12872 assertNull(calibrator.getEstimatedMzx());
12873 assertNull(calibrator.getEstimatedMzy());
12874 assertNull(calibrator.getEstimatedCovariance());
12875 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12876 assertNotNull(calibrator.getGroundTruthGravityNorm());
12877 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12878 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12879 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12880 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12881 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12882 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12883 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12884
12885
12886 calibrator = null;
12887 try {
12888 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12889 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz,
12890 mxy, mxz, myx, myz, mzx, mzy);
12891 fail("IllegalArgumentException expected but not thrown");
12892 } catch (final IllegalArgumentException ignore) {
12893 }
12894 assertNull(calibrator);
12895 }
12896
12897 @Test
12898 public void testConstructor119() throws WrongSizeException {
12899 final Collection<StandardDeviationBodyKinematics> measurements =
12900 Collections.emptyList();
12901
12902 final Matrix ba = generateBa();
12903 final double biasX = ba.getElementAtIndex(0);
12904 final double biasY = ba.getElementAtIndex(1);
12905 final double biasZ = ba.getElementAtIndex(2);
12906
12907 final Matrix ma = generateMaCommonAxis();
12908 final double sx = ma.getElementAt(0, 0);
12909 final double sy = ma.getElementAt(1, 1);
12910 final double sz = ma.getElementAt(2, 2);
12911 final double mxy = ma.getElementAt(0, 1);
12912 final double mxz = ma.getElementAt(0, 2);
12913 final double myx = ma.getElementAt(1, 0);
12914 final double myz = ma.getElementAt(1, 2);
12915 final double mzx = ma.getElementAt(2, 0);
12916 final double mzy = ma.getElementAt(2, 1);
12917
12918 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12919 final double latitude = Math.toRadians(
12920 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12921 final double longitude = Math.toRadians(
12922 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12923 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12924 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12925 final NEDVelocity nedVelocity = new NEDVelocity();
12926 final ECEFPosition ecefPosition = new ECEFPosition();
12927 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12928 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12929 ecefPosition, ecefVelocity);
12930 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12931 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12932 final double gravityNorm = gravity.getNorm();
12933
12934 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12935 new KnownBiasAndGravityNormAccelerometerCalibrator(
12936 gravityNorm, measurements,
12937 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
12938 myx, myz, mzx, mzy);
12939
12940
12941 assertEquals(calibrator.getBiasX(), biasX, 0.0);
12942 assertEquals(calibrator.getBiasY(), biasY, 0.0);
12943 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12944 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12945 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12946 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12947 final Acceleration bx2 = new Acceleration(0.0,
12948 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12949 calibrator.getBiasXAsAcceleration(bx2);
12950 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12951 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12952 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12953 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12954 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12955 final Acceleration by2 = new Acceleration(0.0,
12956 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12957 calibrator.getBiasYAsAcceleration(by2);
12958 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12959 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12960 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12961 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12962 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12963 final Acceleration bz2 = new Acceleration(0.0,
12964 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12965 calibrator.getBiasZAsAcceleration(bz2);
12966 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12967 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12968 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12969 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12970 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12971 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
12972 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
12973 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
12974 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
12975 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
12976 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
12977 final double[] bias1 = calibrator.getBias();
12978 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12979 final double[] bias2 = new double[3];
12980 calibrator.getBias(bias2);
12981 assertArrayEquals(bias1, bias2, 0.0);
12982 final Matrix b1 = calibrator.getBiasAsMatrix();
12983 assertEquals(b1, ba);
12984 final Matrix b2 = new Matrix(3, 1);
12985 calibrator.getBiasAsMatrix(b2);
12986 assertEquals(b1, b2);
12987 final Matrix ma1 = new Matrix(3, 3);
12988 ma1.setSubmatrix(0, 0,
12989 2, 2,
12990 new double[]{sx, myx, mzx,
12991 mxy, sy, mzy,
12992 mxz, myz, sz});
12993 assertEquals(calibrator.getInitialMa(), ma1);
12994 final Matrix ma2 = new Matrix(3, 3);
12995 calibrator.getInitialMa(ma2);
12996 assertEquals(ma1, ma2);
12997 assertSame(calibrator.getMeasurements(), measurements);
12998 assertFalse(calibrator.isCommonAxisUsed());
12999 assertNull(calibrator.getListener());
13000 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13001 assertFalse(calibrator.isReady());
13002 assertFalse(calibrator.isRunning());
13003 assertNull(calibrator.getEstimatedMa());
13004 assertNull(calibrator.getEstimatedSx());
13005 assertNull(calibrator.getEstimatedSy());
13006 assertNull(calibrator.getEstimatedSz());
13007 assertNull(calibrator.getEstimatedMxy());
13008 assertNull(calibrator.getEstimatedMxz());
13009 assertNull(calibrator.getEstimatedMyx());
13010 assertNull(calibrator.getEstimatedMyz());
13011 assertNull(calibrator.getEstimatedMzx());
13012 assertNull(calibrator.getEstimatedMzy());
13013 assertNull(calibrator.getEstimatedCovariance());
13014 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13015 assertNotNull(calibrator.getGroundTruthGravityNorm());
13016 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13017 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13018 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13019 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13020 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13021 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13022 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13023
13024
13025 calibrator = null;
13026 try {
13027 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13028 -gravityNorm, measurements,
13029 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13030 myx, myz, mzx, mzy);
13031 fail("IllegalArgumentException expected but not thrown");
13032 } catch (final IllegalArgumentException ignore) {
13033 }
13034 assertNull(calibrator);
13035 }
13036
13037 @Test
13038 public void testConstructor120() throws WrongSizeException {
13039 final Collection<StandardDeviationBodyKinematics> measurements =
13040 Collections.emptyList();
13041
13042 final Matrix ba = generateBa();
13043 final double biasX = ba.getElementAtIndex(0);
13044 final double biasY = ba.getElementAtIndex(1);
13045 final double biasZ = ba.getElementAtIndex(2);
13046
13047 final Matrix ma = generateMaCommonAxis();
13048 final double sx = ma.getElementAt(0, 0);
13049 final double sy = ma.getElementAt(1, 1);
13050 final double sz = ma.getElementAt(2, 2);
13051 final double mxy = ma.getElementAt(0, 1);
13052 final double mxz = ma.getElementAt(0, 2);
13053 final double myx = ma.getElementAt(1, 0);
13054 final double myz = ma.getElementAt(1, 2);
13055 final double mzx = ma.getElementAt(2, 0);
13056 final double mzy = ma.getElementAt(2, 1);
13057
13058 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13059 final double latitude = Math.toRadians(
13060 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13061 final double longitude = Math.toRadians(
13062 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13063 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13064 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13065 final NEDVelocity nedVelocity = new NEDVelocity();
13066 final ECEFPosition ecefPosition = new ECEFPosition();
13067 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13068 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13069 ecefPosition, ecefVelocity);
13070 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13071 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13072 final double gravityNorm = gravity.getNorm();
13073
13074 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13075 new KnownBiasAndGravityNormAccelerometerCalibrator(
13076 gravityNorm, measurements,
13077 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13078 myx, myz, mzx, mzy, this);
13079
13080
13081 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13082 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13083 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13084 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13085 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13086 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13087 final Acceleration bx2 = new Acceleration(0.0,
13088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13089 calibrator.getBiasXAsAcceleration(bx2);
13090 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13091 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13092 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13093 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13094 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13095 final Acceleration by2 = new Acceleration(0.0,
13096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13097 calibrator.getBiasYAsAcceleration(by2);
13098 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13099 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13100 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13101 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13102 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13103 final Acceleration bz2 = new Acceleration(0.0,
13104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13105 calibrator.getBiasZAsAcceleration(bz2);
13106 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13107 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13108 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13109 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13110 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13111 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13112 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13113 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13114 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13115 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13116 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13117 final double[] bias1 = calibrator.getBias();
13118 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13119 final double[] bias2 = new double[3];
13120 calibrator.getBias(bias2);
13121 assertArrayEquals(bias1, bias2, 0.0);
13122 final Matrix b1 = calibrator.getBiasAsMatrix();
13123 assertEquals(b1, ba);
13124 final Matrix b2 = new Matrix(3, 1);
13125 calibrator.getBiasAsMatrix(b2);
13126 assertEquals(b1, b2);
13127 final Matrix ma1 = new Matrix(3, 3);
13128 ma1.setSubmatrix(0, 0,
13129 2, 2,
13130 new double[]{sx, myx, mzx,
13131 mxy, sy, mzy,
13132 mxz, myz, sz});
13133 assertEquals(calibrator.getInitialMa(), ma1);
13134 final Matrix ma2 = new Matrix(3, 3);
13135 calibrator.getInitialMa(ma2);
13136 assertEquals(ma1, ma2);
13137 assertSame(calibrator.getMeasurements(), measurements);
13138 assertFalse(calibrator.isCommonAxisUsed());
13139 assertSame(calibrator.getListener(), this);
13140 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13141 assertFalse(calibrator.isReady());
13142 assertFalse(calibrator.isRunning());
13143 assertNull(calibrator.getEstimatedMa());
13144 assertNull(calibrator.getEstimatedSx());
13145 assertNull(calibrator.getEstimatedSy());
13146 assertNull(calibrator.getEstimatedSz());
13147 assertNull(calibrator.getEstimatedMxy());
13148 assertNull(calibrator.getEstimatedMxz());
13149 assertNull(calibrator.getEstimatedMyx());
13150 assertNull(calibrator.getEstimatedMyz());
13151 assertNull(calibrator.getEstimatedMzx());
13152 assertNull(calibrator.getEstimatedMzy());
13153 assertNull(calibrator.getEstimatedCovariance());
13154 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13155 assertNotNull(calibrator.getGroundTruthGravityNorm());
13156 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13157 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13158 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13159 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13160 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13161 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13162 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13163
13164
13165 calibrator = null;
13166 try {
13167 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13168 -gravityNorm, measurements,
13169 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13170 myx, myz, mzx, mzy, this);
13171 fail("IllegalArgumentException expected but not thrown");
13172 } catch (final IllegalArgumentException ignore) {
13173 }
13174 assertNull(calibrator);
13175 }
13176
13177 @Test
13178 public void testConstructor121() throws WrongSizeException {
13179 final Matrix ba = generateBa();
13180 final double biasX = ba.getElementAtIndex(0);
13181 final double biasY = ba.getElementAtIndex(1);
13182 final double biasZ = ba.getElementAtIndex(2);
13183
13184 final Matrix ma = generateMaCommonAxis();
13185 final double sx = ma.getElementAt(0, 0);
13186 final double sy = ma.getElementAt(1, 1);
13187 final double sz = ma.getElementAt(2, 2);
13188 final double mxy = ma.getElementAt(0, 1);
13189 final double mxz = ma.getElementAt(0, 2);
13190 final double myx = ma.getElementAt(1, 0);
13191 final double myz = ma.getElementAt(1, 2);
13192 final double mzx = ma.getElementAt(2, 0);
13193 final double mzy = ma.getElementAt(2, 1);
13194
13195 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13196 final double latitude = Math.toRadians(
13197 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13198 final double longitude = Math.toRadians(
13199 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13200 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13201 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13202 final NEDVelocity nedVelocity = new NEDVelocity();
13203 final ECEFPosition ecefPosition = new ECEFPosition();
13204 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13205 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13206 ecefPosition, ecefVelocity);
13207 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13208 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13209 final double gravityNorm = gravity.getNorm();
13210
13211 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13212 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13213 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13214 myx, myz, mzx, mzy);
13215
13216
13217 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13218 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13219 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13220 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13221 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13222 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13223 final Acceleration bx2 = new Acceleration(0.0,
13224 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13225 calibrator.getBiasXAsAcceleration(bx2);
13226 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13227 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13228 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13229 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13230 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13231 final Acceleration by2 = new Acceleration(0.0,
13232 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13233 calibrator.getBiasYAsAcceleration(by2);
13234 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13235 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13236 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13237 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13238 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13239 final Acceleration bz2 = new Acceleration(0.0,
13240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13241 calibrator.getBiasZAsAcceleration(bz2);
13242 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13243 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13244 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13245 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13246 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13247 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13248 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13249 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13250 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13251 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13252 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13253 final double[] bias1 = calibrator.getBias();
13254 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13255 final double[] bias2 = new double[3];
13256 calibrator.getBias(bias2);
13257 assertArrayEquals(bias1, bias2, 0.0);
13258 final Matrix b1 = calibrator.getBiasAsMatrix();
13259 assertEquals(b1, ba);
13260 final Matrix b2 = new Matrix(3, 1);
13261 calibrator.getBiasAsMatrix(b2);
13262 assertEquals(b1, b2);
13263 final Matrix ma1 = new Matrix(3, 3);
13264 ma1.setSubmatrix(0, 0,
13265 2, 2,
13266 new double[]{sx, myx, mzx,
13267 mxy, sy, mzy,
13268 mxz, myz, sz});
13269 assertEquals(calibrator.getInitialMa(), ma1);
13270 final Matrix ma2 = new Matrix(3, 3);
13271 calibrator.getInitialMa(ma2);
13272 assertEquals(ma1, ma2);
13273 assertNull(calibrator.getMeasurements());
13274 assertTrue(calibrator.isCommonAxisUsed());
13275 assertNull(calibrator.getListener());
13276 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13277 assertFalse(calibrator.isReady());
13278 assertFalse(calibrator.isRunning());
13279 assertNull(calibrator.getEstimatedMa());
13280 assertNull(calibrator.getEstimatedSx());
13281 assertNull(calibrator.getEstimatedSy());
13282 assertNull(calibrator.getEstimatedSz());
13283 assertNull(calibrator.getEstimatedMxy());
13284 assertNull(calibrator.getEstimatedMxz());
13285 assertNull(calibrator.getEstimatedMyx());
13286 assertNull(calibrator.getEstimatedMyz());
13287 assertNull(calibrator.getEstimatedMzx());
13288 assertNull(calibrator.getEstimatedMzy());
13289 assertNull(calibrator.getEstimatedCovariance());
13290 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13291 assertNotNull(calibrator.getGroundTruthGravityNorm());
13292 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13293 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13294 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13295 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13296 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13297 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13298 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13299
13300
13301 calibrator = null;
13302 try {
13303 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13304 -gravityNorm, true, biasX, biasY, biasZ,
13305 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13306 fail("IllegalArgumentException expected but not thrown");
13307 } catch (final IllegalArgumentException ignore) {
13308 }
13309 assertNull(calibrator);
13310 }
13311
13312 @Test
13313 public void testConstructor122() throws WrongSizeException {
13314 final Matrix ba = generateBa();
13315 final double biasX = ba.getElementAtIndex(0);
13316 final double biasY = ba.getElementAtIndex(1);
13317 final double biasZ = ba.getElementAtIndex(2);
13318
13319 final Matrix ma = generateMaCommonAxis();
13320 final double sx = ma.getElementAt(0, 0);
13321 final double sy = ma.getElementAt(1, 1);
13322 final double sz = ma.getElementAt(2, 2);
13323 final double mxy = ma.getElementAt(0, 1);
13324 final double mxz = ma.getElementAt(0, 2);
13325 final double myx = ma.getElementAt(1, 0);
13326 final double myz = ma.getElementAt(1, 2);
13327 final double mzx = ma.getElementAt(2, 0);
13328 final double mzy = ma.getElementAt(2, 1);
13329
13330 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13331 final double latitude = Math.toRadians(
13332 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13333 final double longitude = Math.toRadians(
13334 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13335 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13336 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13337 final NEDVelocity nedVelocity = new NEDVelocity();
13338 final ECEFPosition ecefPosition = new ECEFPosition();
13339 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13340 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13341 ecefPosition, ecefVelocity);
13342 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13343 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13344 final double gravityNorm = gravity.getNorm();
13345
13346 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13347 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13348 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13349 myx, myz, mzx, mzy, this);
13350
13351
13352 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13353 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13354 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13355 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13356 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13357 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13358 final Acceleration bx2 = new Acceleration(0.0,
13359 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13360 calibrator.getBiasXAsAcceleration(bx2);
13361 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13362 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13363 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13364 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13365 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13366 final Acceleration by2 = new Acceleration(0.0,
13367 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13368 calibrator.getBiasYAsAcceleration(by2);
13369 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13370 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13371 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13372 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13373 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13374 final Acceleration bz2 = new Acceleration(0.0,
13375 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13376 calibrator.getBiasZAsAcceleration(bz2);
13377 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13378 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13379 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13380 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13381 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13382 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13383 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13384 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13385 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13386 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13387 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13388 final double[] bias1 = calibrator.getBias();
13389 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13390 final double[] bias2 = new double[3];
13391 calibrator.getBias(bias2);
13392 assertArrayEquals(bias1, bias2, 0.0);
13393 final Matrix b1 = calibrator.getBiasAsMatrix();
13394 assertEquals(b1, ba);
13395 final Matrix b2 = new Matrix(3, 1);
13396 calibrator.getBiasAsMatrix(b2);
13397 assertEquals(b1, b2);
13398 final Matrix ma1 = new Matrix(3, 3);
13399 ma1.setSubmatrix(0, 0,
13400 2, 2,
13401 new double[]{sx, myx, mzx,
13402 mxy, sy, mzy,
13403 mxz, myz, sz});
13404 assertEquals(calibrator.getInitialMa(), ma1);
13405 final Matrix ma2 = new Matrix(3, 3);
13406 calibrator.getInitialMa(ma2);
13407 assertEquals(ma1, ma2);
13408 assertNull(calibrator.getMeasurements());
13409 assertTrue(calibrator.isCommonAxisUsed());
13410 assertSame(calibrator.getListener(), this);
13411 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13412 assertFalse(calibrator.isReady());
13413 assertFalse(calibrator.isRunning());
13414 assertNull(calibrator.getEstimatedMa());
13415 assertNull(calibrator.getEstimatedSx());
13416 assertNull(calibrator.getEstimatedSy());
13417 assertNull(calibrator.getEstimatedSz());
13418 assertNull(calibrator.getEstimatedMxy());
13419 assertNull(calibrator.getEstimatedMxz());
13420 assertNull(calibrator.getEstimatedMyx());
13421 assertNull(calibrator.getEstimatedMyz());
13422 assertNull(calibrator.getEstimatedMzx());
13423 assertNull(calibrator.getEstimatedMzy());
13424 assertNull(calibrator.getEstimatedCovariance());
13425 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13426 assertNotNull(calibrator.getGroundTruthGravityNorm());
13427 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13428 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13429 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13430 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13431 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13432 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13433 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13434
13435
13436 calibrator = null;
13437 try {
13438 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13439 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
13440 mxy, mxz, myx, myz, mzx, mzy, this);
13441 fail("IllegalArgumentException expected but not thrown");
13442 } catch (final IllegalArgumentException ignore) {
13443 }
13444 assertNull(calibrator);
13445 }
13446
13447 @Test
13448 public void testConstructor123() throws WrongSizeException {
13449 final Collection<StandardDeviationBodyKinematics> measurements =
13450 Collections.emptyList();
13451
13452 final Matrix ba = generateBa();
13453 final double biasX = ba.getElementAtIndex(0);
13454 final double biasY = ba.getElementAtIndex(1);
13455 final double biasZ = ba.getElementAtIndex(2);
13456
13457 final Matrix ma = generateMaCommonAxis();
13458 final double sx = ma.getElementAt(0, 0);
13459 final double sy = ma.getElementAt(1, 1);
13460 final double sz = ma.getElementAt(2, 2);
13461 final double mxy = ma.getElementAt(0, 1);
13462 final double mxz = ma.getElementAt(0, 2);
13463 final double myx = ma.getElementAt(1, 0);
13464 final double myz = ma.getElementAt(1, 2);
13465 final double mzx = ma.getElementAt(2, 0);
13466 final double mzy = ma.getElementAt(2, 1);
13467
13468 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13469 final double latitude = Math.toRadians(
13470 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13471 final double longitude = Math.toRadians(
13472 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13473 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13474 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13475 final NEDVelocity nedVelocity = new NEDVelocity();
13476 final ECEFPosition ecefPosition = new ECEFPosition();
13477 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13478 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13479 ecefPosition, ecefVelocity);
13480 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13481 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13482 final double gravityNorm = gravity.getNorm();
13483
13484 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13485 new KnownBiasAndGravityNormAccelerometerCalibrator(
13486 gravityNorm, measurements,
13487 true, biasX, biasY, biasZ,
13488 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13489
13490
13491 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13492 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13493 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13494 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13495 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13496 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13497 final Acceleration bx2 = new Acceleration(0.0,
13498 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13499 calibrator.getBiasXAsAcceleration(bx2);
13500 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13501 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13502 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13503 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13504 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13505 final Acceleration by2 = new Acceleration(0.0,
13506 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13507 calibrator.getBiasYAsAcceleration(by2);
13508 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13509 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13510 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13511 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13512 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13513 final Acceleration bz2 = new Acceleration(0.0,
13514 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13515 calibrator.getBiasZAsAcceleration(bz2);
13516 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13517 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13518 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13519 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13520 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13521 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13522 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13523 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13524 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13525 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13526 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13527 final double[] bias1 = calibrator.getBias();
13528 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13529 final double[] bias2 = new double[3];
13530 calibrator.getBias(bias2);
13531 assertArrayEquals(bias1, bias2, 0.0);
13532 final Matrix b1 = calibrator.getBiasAsMatrix();
13533 assertEquals(b1, ba);
13534 final Matrix b2 = new Matrix(3, 1);
13535 calibrator.getBiasAsMatrix(b2);
13536 assertEquals(b1, b2);
13537 final Matrix ma1 = new Matrix(3, 3);
13538 ma1.setSubmatrix(0, 0,
13539 2, 2,
13540 new double[]{sx, myx, mzx,
13541 mxy, sy, mzy,
13542 mxz, myz, sz});
13543 assertEquals(calibrator.getInitialMa(), ma1);
13544 final Matrix ma2 = new Matrix(3, 3);
13545 calibrator.getInitialMa(ma2);
13546 assertEquals(ma1, ma2);
13547 assertSame(calibrator.getMeasurements(), measurements);
13548 assertTrue(calibrator.isCommonAxisUsed());
13549 assertNull(calibrator.getListener());
13550 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13551 assertFalse(calibrator.isReady());
13552 assertFalse(calibrator.isRunning());
13553 assertNull(calibrator.getEstimatedMa());
13554 assertNull(calibrator.getEstimatedSx());
13555 assertNull(calibrator.getEstimatedSy());
13556 assertNull(calibrator.getEstimatedSz());
13557 assertNull(calibrator.getEstimatedMxy());
13558 assertNull(calibrator.getEstimatedMxz());
13559 assertNull(calibrator.getEstimatedMyx());
13560 assertNull(calibrator.getEstimatedMyz());
13561 assertNull(calibrator.getEstimatedMzx());
13562 assertNull(calibrator.getEstimatedMzy());
13563 assertNull(calibrator.getEstimatedCovariance());
13564 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13565 assertNotNull(calibrator.getGroundTruthGravityNorm());
13566 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13567 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13568 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13569 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13570 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13571 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13572 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13573
13574
13575 calibrator = null;
13576 try {
13577 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13578 -gravityNorm, measurements,
13579 true, biasX, biasY, biasZ,
13580 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13581 fail("IllegalArgumentException expected but not thrown");
13582 } catch (final IllegalArgumentException ignore) {
13583 }
13584 assertNull(calibrator);
13585 }
13586
13587 @Test
13588 public void testConstructor124() throws WrongSizeException {
13589 final Collection<StandardDeviationBodyKinematics> measurements =
13590 Collections.emptyList();
13591
13592 final Matrix ba = generateBa();
13593 final double biasX = ba.getElementAtIndex(0);
13594 final double biasY = ba.getElementAtIndex(1);
13595 final double biasZ = ba.getElementAtIndex(2);
13596
13597 final Matrix ma = generateMaCommonAxis();
13598 final double sx = ma.getElementAt(0, 0);
13599 final double sy = ma.getElementAt(1, 1);
13600 final double sz = ma.getElementAt(2, 2);
13601 final double mxy = ma.getElementAt(0, 1);
13602 final double mxz = ma.getElementAt(0, 2);
13603 final double myx = ma.getElementAt(1, 0);
13604 final double myz = ma.getElementAt(1, 2);
13605 final double mzx = ma.getElementAt(2, 0);
13606 final double mzy = ma.getElementAt(2, 1);
13607
13608 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13609 final double latitude = Math.toRadians(
13610 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13611 final double longitude = Math.toRadians(
13612 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13613 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13614 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13615 final NEDVelocity nedVelocity = new NEDVelocity();
13616 final ECEFPosition ecefPosition = new ECEFPosition();
13617 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13618 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13619 ecefPosition, ecefVelocity);
13620 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13621 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13622 final double gravityNorm = gravity.getNorm();
13623
13624 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13625 new KnownBiasAndGravityNormAccelerometerCalibrator(
13626 gravityNorm, measurements,
13627 true, biasX, biasY, biasZ,
13628 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13629 this);
13630
13631
13632 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13633 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13634 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13635 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13636 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13637 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13638 final Acceleration bx2 = new Acceleration(0.0,
13639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13640 calibrator.getBiasXAsAcceleration(bx2);
13641 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13642 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13643 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13644 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13645 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13646 final Acceleration by2 = new Acceleration(0.0,
13647 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13648 calibrator.getBiasYAsAcceleration(by2);
13649 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13650 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13651 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13652 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13653 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13654 final Acceleration bz2 = new Acceleration(0.0,
13655 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13656 calibrator.getBiasZAsAcceleration(bz2);
13657 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13658 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13659 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13660 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13661 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13662 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13663 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13664 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13665 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13666 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13667 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13668 final double[] bias1 = calibrator.getBias();
13669 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13670 final double[] bias2 = new double[3];
13671 calibrator.getBias(bias2);
13672 assertArrayEquals(bias1, bias2, 0.0);
13673 final Matrix b1 = calibrator.getBiasAsMatrix();
13674 assertEquals(b1, ba);
13675 final Matrix b2 = new Matrix(3, 1);
13676 calibrator.getBiasAsMatrix(b2);
13677 assertEquals(b1, b2);
13678 final Matrix ma1 = new Matrix(3, 3);
13679 ma1.setSubmatrix(0, 0,
13680 2, 2,
13681 new double[]{sx, myx, mzx,
13682 mxy, sy, mzy,
13683 mxz, myz, sz});
13684 assertEquals(calibrator.getInitialMa(), ma1);
13685 final Matrix ma2 = new Matrix(3, 3);
13686 calibrator.getInitialMa(ma2);
13687 assertEquals(ma1, ma2);
13688 assertSame(calibrator.getMeasurements(), measurements);
13689 assertTrue(calibrator.isCommonAxisUsed());
13690 assertSame(calibrator.getListener(), this);
13691 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13692 assertFalse(calibrator.isReady());
13693 assertFalse(calibrator.isRunning());
13694 assertNull(calibrator.getEstimatedMa());
13695 assertNull(calibrator.getEstimatedSx());
13696 assertNull(calibrator.getEstimatedSy());
13697 assertNull(calibrator.getEstimatedSz());
13698 assertNull(calibrator.getEstimatedMxy());
13699 assertNull(calibrator.getEstimatedMxz());
13700 assertNull(calibrator.getEstimatedMyx());
13701 assertNull(calibrator.getEstimatedMyz());
13702 assertNull(calibrator.getEstimatedMzx());
13703 assertNull(calibrator.getEstimatedMzy());
13704 assertNull(calibrator.getEstimatedCovariance());
13705 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13706 assertNotNull(calibrator.getGroundTruthGravityNorm());
13707 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13708 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13709 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13710 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13711 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13712 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13713 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13714
13715
13716 calibrator = null;
13717 try {
13718 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13719 -gravityNorm, measurements,
13720 true, biasX, biasY, biasZ,
13721 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13722 this);
13723 fail("IllegalArgumentException expected but not thrown");
13724 } catch (final IllegalArgumentException ignore) {
13725 }
13726 assertNull(calibrator);
13727 }
13728
13729 @Test
13730 public void testConstructor125() throws WrongSizeException {
13731 final Matrix ba = generateBa();
13732 final double biasX = ba.getElementAtIndex(0);
13733 final double biasY = ba.getElementAtIndex(1);
13734 final double biasZ = ba.getElementAtIndex(2);
13735
13736 final Acceleration bx = new Acceleration(biasX,
13737 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13738 final Acceleration by = new Acceleration(biasY,
13739 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13740 final Acceleration bz = new Acceleration(biasZ,
13741 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13742
13743 final Matrix ma = generateMaCommonAxis();
13744 final double sx = ma.getElementAt(0, 0);
13745 final double sy = ma.getElementAt(1, 1);
13746 final double sz = ma.getElementAt(2, 2);
13747 final double mxy = ma.getElementAt(0, 1);
13748 final double mxz = ma.getElementAt(0, 2);
13749 final double myx = ma.getElementAt(1, 0);
13750 final double myz = ma.getElementAt(1, 2);
13751 final double mzx = ma.getElementAt(2, 0);
13752 final double mzy = ma.getElementAt(2, 1);
13753
13754 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13755 final double latitude = Math.toRadians(
13756 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13757 final double longitude = Math.toRadians(
13758 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13759 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13760 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13761 final NEDVelocity nedVelocity = new NEDVelocity();
13762 final ECEFPosition ecefPosition = new ECEFPosition();
13763 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13764 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13765 ecefPosition, ecefVelocity);
13766 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13767 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13768 final double gravityNorm = gravity.getNorm();
13769
13770 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13771 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13772 bx, by, bz, sx, sy, sz, mxy, mxz,
13773 myx, myz, mzx, mzy);
13774
13775
13776 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13777 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13778 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13779 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13780 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13781 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13782 final Acceleration bx2 = new Acceleration(0.0,
13783 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13784 calibrator.getBiasXAsAcceleration(bx2);
13785 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13786 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13787 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13788 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13789 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13790 final Acceleration by2 = new Acceleration(0.0,
13791 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13792 calibrator.getBiasYAsAcceleration(by2);
13793 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13794 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13795 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13796 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13797 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13798 final Acceleration bz2 = new Acceleration(0.0,
13799 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13800 calibrator.getBiasZAsAcceleration(bz2);
13801 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13802 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13803 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13804 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13805 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13806 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13807 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13808 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13809 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13810 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13811 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13812 final double[] bias1 = calibrator.getBias();
13813 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13814 final double[] bias2 = new double[3];
13815 calibrator.getBias(bias2);
13816 assertArrayEquals(bias1, bias2, 0.0);
13817 final Matrix b1 = calibrator.getBiasAsMatrix();
13818 assertEquals(b1, ba);
13819 final Matrix b2 = new Matrix(3, 1);
13820 calibrator.getBiasAsMatrix(b2);
13821 assertEquals(b1, b2);
13822 final Matrix ma1 = new Matrix(3, 3);
13823 ma1.setSubmatrix(0, 0,
13824 2, 2,
13825 new double[]{sx, myx, mzx,
13826 mxy, sy, mzy,
13827 mxz, myz, sz});
13828 assertEquals(calibrator.getInitialMa(), ma1);
13829 final Matrix ma2 = new Matrix(3, 3);
13830 calibrator.getInitialMa(ma2);
13831 assertEquals(ma1, ma2);
13832 assertNull(calibrator.getMeasurements());
13833 assertFalse(calibrator.isCommonAxisUsed());
13834 assertNull(calibrator.getListener());
13835 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13836 assertFalse(calibrator.isReady());
13837 assertFalse(calibrator.isRunning());
13838 assertNull(calibrator.getEstimatedMa());
13839 assertNull(calibrator.getEstimatedSx());
13840 assertNull(calibrator.getEstimatedSy());
13841 assertNull(calibrator.getEstimatedSz());
13842 assertNull(calibrator.getEstimatedMxy());
13843 assertNull(calibrator.getEstimatedMxz());
13844 assertNull(calibrator.getEstimatedMyx());
13845 assertNull(calibrator.getEstimatedMyz());
13846 assertNull(calibrator.getEstimatedMzx());
13847 assertNull(calibrator.getEstimatedMzy());
13848 assertNull(calibrator.getEstimatedCovariance());
13849 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13850 assertNotNull(calibrator.getGroundTruthGravityNorm());
13851 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13852 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13853 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13854 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13855 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13856 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13857 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13858
13859
13860 calibrator = null;
13861 try {
13862 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13863 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
13864 myx, myz, mzx, mzy);
13865 fail("IllegalArgumentException expected but not thrown");
13866 } catch (final IllegalArgumentException ignore) {
13867 }
13868 assertNull(calibrator);
13869 }
13870
13871 @Test
13872 public void testConstructor126() throws WrongSizeException {
13873 final Matrix ba = generateBa();
13874 final double biasX = ba.getElementAtIndex(0);
13875 final double biasY = ba.getElementAtIndex(1);
13876 final double biasZ = ba.getElementAtIndex(2);
13877
13878 final Acceleration bx = new Acceleration(biasX,
13879 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13880 final Acceleration by = new Acceleration(biasY,
13881 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13882 final Acceleration bz = new Acceleration(biasZ,
13883 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13884
13885 final Matrix ma = generateMaCommonAxis();
13886 final double sx = ma.getElementAt(0, 0);
13887 final double sy = ma.getElementAt(1, 1);
13888 final double sz = ma.getElementAt(2, 2);
13889 final double mxy = ma.getElementAt(0, 1);
13890 final double mxz = ma.getElementAt(0, 2);
13891 final double myx = ma.getElementAt(1, 0);
13892 final double myz = ma.getElementAt(1, 2);
13893 final double mzx = ma.getElementAt(2, 0);
13894 final double mzy = ma.getElementAt(2, 1);
13895
13896 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13897 final double latitude = Math.toRadians(
13898 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13899 final double longitude = Math.toRadians(
13900 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13901 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13902 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13903 final NEDVelocity nedVelocity = new NEDVelocity();
13904 final ECEFPosition ecefPosition = new ECEFPosition();
13905 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13906 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13907 ecefPosition, ecefVelocity);
13908 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13909 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13910 final double gravityNorm = gravity.getNorm();
13911
13912 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13913 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13914 bx, by, bz, sx, sy, sz, mxy, mxz,
13915 myx, myz, mzx, mzy, this);
13916
13917
13918 assertEquals(calibrator.getBiasX(), biasX, 0.0);
13919 assertEquals(calibrator.getBiasY(), biasY, 0.0);
13920 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13921 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13922 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13923 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13924 final Acceleration bx2 = new Acceleration(0.0,
13925 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13926 calibrator.getBiasXAsAcceleration(bx2);
13927 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13928 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13929 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13930 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13931 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13932 final Acceleration by2 = new Acceleration(0.0,
13933 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13934 calibrator.getBiasYAsAcceleration(by2);
13935 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13936 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13937 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13938 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13939 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13940 final Acceleration bz2 = new Acceleration(0.0,
13941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13942 calibrator.getBiasZAsAcceleration(bz2);
13943 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13944 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13945 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13946 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13947 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13948 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13949 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13950 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13951 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13952 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13953 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13954 final double[] bias1 = calibrator.getBias();
13955 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13956 final double[] bias2 = new double[3];
13957 calibrator.getBias(bias2);
13958 assertArrayEquals(bias1, bias2, 0.0);
13959 final Matrix b1 = calibrator.getBiasAsMatrix();
13960 assertEquals(b1, ba);
13961 final Matrix b2 = new Matrix(3, 1);
13962 calibrator.getBiasAsMatrix(b2);
13963 assertEquals(b1, b2);
13964 final Matrix ma1 = new Matrix(3, 3);
13965 ma1.setSubmatrix(0, 0,
13966 2, 2,
13967 new double[]{sx, myx, mzx,
13968 mxy, sy, mzy,
13969 mxz, myz, sz});
13970 assertEquals(calibrator.getInitialMa(), ma1);
13971 final Matrix ma2 = new Matrix(3, 3);
13972 calibrator.getInitialMa(ma2);
13973 assertEquals(ma1, ma2);
13974 assertNull(calibrator.getMeasurements());
13975 assertFalse(calibrator.isCommonAxisUsed());
13976 assertSame(calibrator.getListener(), this);
13977 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13978 assertFalse(calibrator.isReady());
13979 assertFalse(calibrator.isRunning());
13980 assertNull(calibrator.getEstimatedMa());
13981 assertNull(calibrator.getEstimatedSx());
13982 assertNull(calibrator.getEstimatedSy());
13983 assertNull(calibrator.getEstimatedSz());
13984 assertNull(calibrator.getEstimatedMxy());
13985 assertNull(calibrator.getEstimatedMxz());
13986 assertNull(calibrator.getEstimatedMyx());
13987 assertNull(calibrator.getEstimatedMyz());
13988 assertNull(calibrator.getEstimatedMzx());
13989 assertNull(calibrator.getEstimatedMzy());
13990 assertNull(calibrator.getEstimatedCovariance());
13991 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13992 assertNotNull(calibrator.getGroundTruthGravityNorm());
13993 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13994 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13995 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13996 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13997 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13998 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13999 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14000
14001
14002 calibrator = null;
14003 try {
14004 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14005 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
14006 myx, myz, mzx, mzy, this);
14007 fail("IllegalArgumentException expected but not thrown");
14008 } catch (final IllegalArgumentException ignore) {
14009 }
14010 assertNull(calibrator);
14011 }
14012
14013 @Test
14014 public void testConstructor127() throws WrongSizeException {
14015 final Collection<StandardDeviationBodyKinematics> measurements =
14016 Collections.emptyList();
14017
14018 final Matrix ba = generateBa();
14019 final double biasX = ba.getElementAtIndex(0);
14020 final double biasY = ba.getElementAtIndex(1);
14021 final double biasZ = ba.getElementAtIndex(2);
14022
14023 final Acceleration bx = new Acceleration(biasX,
14024 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14025 final Acceleration by = new Acceleration(biasY,
14026 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14027 final Acceleration bz = new Acceleration(biasZ,
14028 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14029
14030 final Matrix ma = generateMaCommonAxis();
14031 final double sx = ma.getElementAt(0, 0);
14032 final double sy = ma.getElementAt(1, 1);
14033 final double sz = ma.getElementAt(2, 2);
14034 final double mxy = ma.getElementAt(0, 1);
14035 final double mxz = ma.getElementAt(0, 2);
14036 final double myx = ma.getElementAt(1, 0);
14037 final double myz = ma.getElementAt(1, 2);
14038 final double mzx = ma.getElementAt(2, 0);
14039 final double mzy = ma.getElementAt(2, 1);
14040
14041 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14042 final double latitude = Math.toRadians(
14043 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14044 final double longitude = Math.toRadians(
14045 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14046 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14047 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14048 final NEDVelocity nedVelocity = new NEDVelocity();
14049 final ECEFPosition ecefPosition = new ECEFPosition();
14050 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14051 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14052 ecefPosition, ecefVelocity);
14053 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14054 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14055 final double gravityNorm = gravity.getNorm();
14056
14057 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14058 new KnownBiasAndGravityNormAccelerometerCalibrator(
14059 gravityNorm, measurements,
14060 bx, by, bz, sx, sy, sz, mxy, mxz,
14061 myx, myz, mzx, mzy);
14062
14063
14064 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14065 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14066 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14067 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14068 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14069 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14070 final Acceleration bx2 = new Acceleration(0.0,
14071 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14072 calibrator.getBiasXAsAcceleration(bx2);
14073 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14074 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14075 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14076 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14077 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14078 final Acceleration by2 = new Acceleration(0.0,
14079 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14080 calibrator.getBiasYAsAcceleration(by2);
14081 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14082 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14083 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14084 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14085 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14086 final Acceleration bz2 = new Acceleration(0.0,
14087 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14088 calibrator.getBiasZAsAcceleration(bz2);
14089 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14090 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14091 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14092 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14093 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14094 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14095 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14096 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14097 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14098 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14099 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14100 final double[] bias1 = calibrator.getBias();
14101 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14102 final double[] bias2 = new double[3];
14103 calibrator.getBias(bias2);
14104 assertArrayEquals(bias1, bias2, 0.0);
14105 final Matrix b1 = calibrator.getBiasAsMatrix();
14106 assertEquals(b1, ba);
14107 final Matrix b2 = new Matrix(3, 1);
14108 calibrator.getBiasAsMatrix(b2);
14109 assertEquals(b1, b2);
14110 final Matrix ma1 = new Matrix(3, 3);
14111 ma1.setSubmatrix(0, 0,
14112 2, 2,
14113 new double[]{sx, myx, mzx,
14114 mxy, sy, mzy,
14115 mxz, myz, sz});
14116 assertEquals(calibrator.getInitialMa(), ma1);
14117 final Matrix ma2 = new Matrix(3, 3);
14118 calibrator.getInitialMa(ma2);
14119 assertEquals(ma1, ma2);
14120 assertSame(calibrator.getMeasurements(), measurements);
14121 assertFalse(calibrator.isCommonAxisUsed());
14122 assertNull(calibrator.getListener());
14123 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14124 assertFalse(calibrator.isReady());
14125 assertFalse(calibrator.isRunning());
14126 assertNull(calibrator.getEstimatedMa());
14127 assertNull(calibrator.getEstimatedSx());
14128 assertNull(calibrator.getEstimatedSy());
14129 assertNull(calibrator.getEstimatedSz());
14130 assertNull(calibrator.getEstimatedMxy());
14131 assertNull(calibrator.getEstimatedMxz());
14132 assertNull(calibrator.getEstimatedMyx());
14133 assertNull(calibrator.getEstimatedMyz());
14134 assertNull(calibrator.getEstimatedMzx());
14135 assertNull(calibrator.getEstimatedMzy());
14136 assertNull(calibrator.getEstimatedCovariance());
14137 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14138 assertNotNull(calibrator.getGroundTruthGravityNorm());
14139 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14140 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14141 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14142 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14143 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14144 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14145 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14146
14147
14148 calibrator = null;
14149 try {
14150 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14151 -gravityNorm, measurements,
14152 bx, by, bz, sx, sy, sz, mxy, mxz,
14153 myx, myz, mzx, mzy);
14154 fail("IllegalArgumentException expected but not thrown");
14155 } catch (final IllegalArgumentException ignore) {
14156 }
14157 assertNull(calibrator);
14158 }
14159
14160 @Test
14161 public void testConstructor128() throws WrongSizeException {
14162 final Collection<StandardDeviationBodyKinematics> measurements =
14163 Collections.emptyList();
14164
14165 final Matrix ba = generateBa();
14166 final double biasX = ba.getElementAtIndex(0);
14167 final double biasY = ba.getElementAtIndex(1);
14168 final double biasZ = ba.getElementAtIndex(2);
14169
14170 final Acceleration bx = new Acceleration(biasX,
14171 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14172 final Acceleration by = new Acceleration(biasY,
14173 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14174 final Acceleration bz = new Acceleration(biasZ,
14175 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14176
14177 final Matrix ma = generateMaCommonAxis();
14178 final double sx = ma.getElementAt(0, 0);
14179 final double sy = ma.getElementAt(1, 1);
14180 final double sz = ma.getElementAt(2, 2);
14181 final double mxy = ma.getElementAt(0, 1);
14182 final double mxz = ma.getElementAt(0, 2);
14183 final double myx = ma.getElementAt(1, 0);
14184 final double myz = ma.getElementAt(1, 2);
14185 final double mzx = ma.getElementAt(2, 0);
14186 final double mzy = ma.getElementAt(2, 1);
14187
14188 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14189 final double latitude = Math.toRadians(
14190 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14191 final double longitude = Math.toRadians(
14192 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14193 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14194 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14195 final NEDVelocity nedVelocity = new NEDVelocity();
14196 final ECEFPosition ecefPosition = new ECEFPosition();
14197 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14198 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14199 ecefPosition, ecefVelocity);
14200 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14201 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14202 final double gravityNorm = gravity.getNorm();
14203
14204 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14205 new KnownBiasAndGravityNormAccelerometerCalibrator(
14206 gravityNorm, measurements,
14207 bx, by, bz, sx, sy, sz, mxy, mxz,
14208 myx, myz, mzx, mzy, this);
14209
14210
14211 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14212 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14213 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14214 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14215 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14216 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14217 final Acceleration bx2 = new Acceleration(0.0,
14218 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14219 calibrator.getBiasXAsAcceleration(bx2);
14220 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14221 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14222 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14223 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14224 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14225 final Acceleration by2 = new Acceleration(0.0,
14226 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14227 calibrator.getBiasYAsAcceleration(by2);
14228 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14229 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14230 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14231 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14232 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14233 final Acceleration bz2 = new Acceleration(0.0,
14234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14235 calibrator.getBiasZAsAcceleration(bz2);
14236 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14237 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14238 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14239 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14240 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14241 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14242 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14243 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14244 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14245 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14246 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14247 final double[] bias1 = calibrator.getBias();
14248 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14249 final double[] bias2 = new double[3];
14250 calibrator.getBias(bias2);
14251 assertArrayEquals(bias1, bias2, 0.0);
14252 final Matrix b1 = calibrator.getBiasAsMatrix();
14253 assertEquals(b1, ba);
14254 final Matrix b2 = new Matrix(3, 1);
14255 calibrator.getBiasAsMatrix(b2);
14256 assertEquals(b1, b2);
14257 final Matrix ma1 = new Matrix(3, 3);
14258 ma1.setSubmatrix(0, 0,
14259 2, 2,
14260 new double[]{sx, myx, mzx,
14261 mxy, sy, mzy,
14262 mxz, myz, sz});
14263 assertEquals(calibrator.getInitialMa(), ma1);
14264 final Matrix ma2 = new Matrix(3, 3);
14265 calibrator.getInitialMa(ma2);
14266 assertEquals(ma1, ma2);
14267 assertSame(calibrator.getMeasurements(), measurements);
14268 assertFalse(calibrator.isCommonAxisUsed());
14269 assertSame(calibrator.getListener(), this);
14270 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14271 assertFalse(calibrator.isReady());
14272 assertFalse(calibrator.isRunning());
14273 assertNull(calibrator.getEstimatedMa());
14274 assertNull(calibrator.getEstimatedSx());
14275 assertNull(calibrator.getEstimatedSy());
14276 assertNull(calibrator.getEstimatedSz());
14277 assertNull(calibrator.getEstimatedMxy());
14278 assertNull(calibrator.getEstimatedMxz());
14279 assertNull(calibrator.getEstimatedMyx());
14280 assertNull(calibrator.getEstimatedMyz());
14281 assertNull(calibrator.getEstimatedMzx());
14282 assertNull(calibrator.getEstimatedMzy());
14283 assertNull(calibrator.getEstimatedCovariance());
14284 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14285 assertNotNull(calibrator.getGroundTruthGravityNorm());
14286 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14287 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14288 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14289 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14290 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14291 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14292 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14293
14294
14295 calibrator = null;
14296 try {
14297 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14298 -gravityNorm, measurements,
14299 bx, by, bz, sx, sy, sz, mxy, mxz,
14300 myx, myz, mzx, mzy, this);
14301 fail("IllegalArgumentException expected but not thrown");
14302 } catch (final IllegalArgumentException ignore) {
14303 }
14304 assertNull(calibrator);
14305 }
14306
14307 @Test
14308 public void testConstructor129() throws WrongSizeException {
14309 final Matrix ba = generateBa();
14310 final double biasX = ba.getElementAtIndex(0);
14311 final double biasY = ba.getElementAtIndex(1);
14312 final double biasZ = ba.getElementAtIndex(2);
14313
14314 final Acceleration bx = new Acceleration(biasX,
14315 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14316 final Acceleration by = new Acceleration(biasY,
14317 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14318 final Acceleration bz = new Acceleration(biasZ,
14319 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14320
14321 final Matrix ma = generateMaCommonAxis();
14322 final double sx = ma.getElementAt(0, 0);
14323 final double sy = ma.getElementAt(1, 1);
14324 final double sz = ma.getElementAt(2, 2);
14325 final double mxy = ma.getElementAt(0, 1);
14326 final double mxz = ma.getElementAt(0, 2);
14327 final double myx = ma.getElementAt(1, 0);
14328 final double myz = ma.getElementAt(1, 2);
14329 final double mzx = ma.getElementAt(2, 0);
14330 final double mzy = ma.getElementAt(2, 1);
14331
14332 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14333 final double latitude = Math.toRadians(
14334 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14335 final double longitude = Math.toRadians(
14336 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14337 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14338 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14339 final NEDVelocity nedVelocity = new NEDVelocity();
14340 final ECEFPosition ecefPosition = new ECEFPosition();
14341 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14342 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14343 ecefPosition, ecefVelocity);
14344 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14345 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14346 final double gravityNorm = gravity.getNorm();
14347
14348 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14349 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
14350 true, bx, by, bz, sx, sy, sz, mxy, mxz,
14351 myx, myz, mzx, mzy);
14352
14353
14354 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14355 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14356 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14357 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14358 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14359 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14360 final Acceleration bx2 = new Acceleration(0.0,
14361 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14362 calibrator.getBiasXAsAcceleration(bx2);
14363 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14364 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14365 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14366 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14367 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14368 final Acceleration by2 = new Acceleration(0.0,
14369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14370 calibrator.getBiasYAsAcceleration(by2);
14371 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14372 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14373 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14374 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14375 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14376 final Acceleration bz2 = new Acceleration(0.0,
14377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14378 calibrator.getBiasZAsAcceleration(bz2);
14379 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14380 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14381 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14382 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14383 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14384 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14385 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14386 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14387 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14388 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14389 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14390 final double[] bias1 = calibrator.getBias();
14391 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14392 final double[] bias2 = new double[3];
14393 calibrator.getBias(bias2);
14394 assertArrayEquals(bias1, bias2, 0.0);
14395 final Matrix b1 = calibrator.getBiasAsMatrix();
14396 assertEquals(b1, ba);
14397 final Matrix b2 = new Matrix(3, 1);
14398 calibrator.getBiasAsMatrix(b2);
14399 assertEquals(b1, b2);
14400 final Matrix ma1 = new Matrix(3, 3);
14401 ma1.setSubmatrix(0, 0,
14402 2, 2,
14403 new double[]{sx, myx, mzx,
14404 mxy, sy, mzy,
14405 mxz, myz, sz});
14406 assertEquals(calibrator.getInitialMa(), ma1);
14407 final Matrix ma2 = new Matrix(3, 3);
14408 calibrator.getInitialMa(ma2);
14409 assertEquals(ma1, ma2);
14410 assertNull(calibrator.getMeasurements());
14411 assertTrue(calibrator.isCommonAxisUsed());
14412 assertNull(calibrator.getListener());
14413 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14414 assertFalse(calibrator.isReady());
14415 assertFalse(calibrator.isRunning());
14416 assertNull(calibrator.getEstimatedMa());
14417 assertNull(calibrator.getEstimatedSx());
14418 assertNull(calibrator.getEstimatedSy());
14419 assertNull(calibrator.getEstimatedSz());
14420 assertNull(calibrator.getEstimatedMxy());
14421 assertNull(calibrator.getEstimatedMxz());
14422 assertNull(calibrator.getEstimatedMyx());
14423 assertNull(calibrator.getEstimatedMyz());
14424 assertNull(calibrator.getEstimatedMzx());
14425 assertNull(calibrator.getEstimatedMzy());
14426 assertNull(calibrator.getEstimatedCovariance());
14427 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14428 assertNotNull(calibrator.getGroundTruthGravityNorm());
14429 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14430 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14431 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14432 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14433 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14434 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14435 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14436
14437
14438 calibrator = null;
14439 try {
14440 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14441 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
14442 myx, myz, mzx, mzy);
14443 fail("IllegalArgumentException expected but not thrown");
14444 } catch (final IllegalArgumentException ignore) {
14445 }
14446 assertNull(calibrator);
14447 }
14448
14449 @Test
14450 public void testConstructor130() throws WrongSizeException {
14451 final Matrix ba = generateBa();
14452 final double biasX = ba.getElementAtIndex(0);
14453 final double biasY = ba.getElementAtIndex(1);
14454 final double biasZ = ba.getElementAtIndex(2);
14455
14456 final Acceleration bx = new Acceleration(biasX,
14457 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14458 final Acceleration by = new Acceleration(biasY,
14459 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14460 final Acceleration bz = new Acceleration(biasZ,
14461 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14462
14463 final Matrix ma = generateMaCommonAxis();
14464 final double sx = ma.getElementAt(0, 0);
14465 final double sy = ma.getElementAt(1, 1);
14466 final double sz = ma.getElementAt(2, 2);
14467 final double mxy = ma.getElementAt(0, 1);
14468 final double mxz = ma.getElementAt(0, 2);
14469 final double myx = ma.getElementAt(1, 0);
14470 final double myz = ma.getElementAt(1, 2);
14471 final double mzx = ma.getElementAt(2, 0);
14472 final double mzy = ma.getElementAt(2, 1);
14473
14474 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14475 final double latitude = Math.toRadians(
14476 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14477 final double longitude = Math.toRadians(
14478 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14479 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14480 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14481 final NEDVelocity nedVelocity = new NEDVelocity();
14482 final ECEFPosition ecefPosition = new ECEFPosition();
14483 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14484 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14485 ecefPosition, ecefVelocity);
14486 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14487 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14488 final double gravityNorm = gravity.getNorm();
14489
14490 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14491 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
14492 true, bx, by, bz, sx, sy, sz, mxy, mxz,
14493 myx, myz, mzx, mzy, this);
14494
14495
14496 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14497 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14498 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14499 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14500 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14501 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14502 final Acceleration bx2 = new Acceleration(0.0,
14503 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14504 calibrator.getBiasXAsAcceleration(bx2);
14505 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14506 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14507 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14508 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14509 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14510 final Acceleration by2 = new Acceleration(0.0,
14511 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14512 calibrator.getBiasYAsAcceleration(by2);
14513 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14514 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14515 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14516 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14517 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14518 final Acceleration bz2 = new Acceleration(0.0,
14519 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14520 calibrator.getBiasZAsAcceleration(bz2);
14521 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14522 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14523 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14524 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14525 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14526 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14527 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14528 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14529 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14530 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14531 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14532 final double[] bias1 = calibrator.getBias();
14533 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14534 final double[] bias2 = new double[3];
14535 calibrator.getBias(bias2);
14536 assertArrayEquals(bias1, bias2, 0.0);
14537 final Matrix b1 = calibrator.getBiasAsMatrix();
14538 assertEquals(b1, ba);
14539 final Matrix b2 = new Matrix(3, 1);
14540 calibrator.getBiasAsMatrix(b2);
14541 assertEquals(b1, b2);
14542 final Matrix ma1 = new Matrix(3, 3);
14543 ma1.setSubmatrix(0, 0,
14544 2, 2,
14545 new double[]{sx, myx, mzx,
14546 mxy, sy, mzy,
14547 mxz, myz, sz});
14548 assertEquals(calibrator.getInitialMa(), ma1);
14549 final Matrix ma2 = new Matrix(3, 3);
14550 calibrator.getInitialMa(ma2);
14551 assertEquals(ma1, ma2);
14552 assertNull(calibrator.getMeasurements());
14553 assertTrue(calibrator.isCommonAxisUsed());
14554 assertSame(calibrator.getListener(), this);
14555 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14556 assertFalse(calibrator.isReady());
14557 assertFalse(calibrator.isRunning());
14558 assertNull(calibrator.getEstimatedMa());
14559 assertNull(calibrator.getEstimatedSx());
14560 assertNull(calibrator.getEstimatedSy());
14561 assertNull(calibrator.getEstimatedSz());
14562 assertNull(calibrator.getEstimatedMxy());
14563 assertNull(calibrator.getEstimatedMxz());
14564 assertNull(calibrator.getEstimatedMyx());
14565 assertNull(calibrator.getEstimatedMyz());
14566 assertNull(calibrator.getEstimatedMzx());
14567 assertNull(calibrator.getEstimatedMzy());
14568 assertNull(calibrator.getEstimatedCovariance());
14569 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14570 assertNotNull(calibrator.getGroundTruthGravityNorm());
14571 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14572 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14573 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14574 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14575 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14576 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14577 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14578
14579
14580 calibrator = null;
14581 try {
14582 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14583 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
14584 myx, myz, mzx, mzy, this);
14585 fail("IllegalArgumentException expected but not thrown");
14586 } catch (final IllegalArgumentException ignore) {
14587 }
14588 assertNull(calibrator);
14589 }
14590
14591 @Test
14592 public void testConstructor131() throws WrongSizeException {
14593 final Collection<StandardDeviationBodyKinematics> measurements =
14594 Collections.emptyList();
14595
14596 final Matrix ba = generateBa();
14597 final double biasX = ba.getElementAtIndex(0);
14598 final double biasY = ba.getElementAtIndex(1);
14599 final double biasZ = ba.getElementAtIndex(2);
14600
14601 final Acceleration bx = new Acceleration(biasX,
14602 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14603 final Acceleration by = new Acceleration(biasY,
14604 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14605 final Acceleration bz = new Acceleration(biasZ,
14606 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14607
14608 final Matrix ma = generateMaCommonAxis();
14609 final double sx = ma.getElementAt(0, 0);
14610 final double sy = ma.getElementAt(1, 1);
14611 final double sz = ma.getElementAt(2, 2);
14612 final double mxy = ma.getElementAt(0, 1);
14613 final double mxz = ma.getElementAt(0, 2);
14614 final double myx = ma.getElementAt(1, 0);
14615 final double myz = ma.getElementAt(1, 2);
14616 final double mzx = ma.getElementAt(2, 0);
14617 final double mzy = ma.getElementAt(2, 1);
14618
14619 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14620 final double latitude = Math.toRadians(
14621 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14622 final double longitude = Math.toRadians(
14623 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14624 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14625 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14626 final NEDVelocity nedVelocity = new NEDVelocity();
14627 final ECEFPosition ecefPosition = new ECEFPosition();
14628 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14629 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14630 ecefPosition, ecefVelocity);
14631 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14632 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14633 final double gravityNorm = gravity.getNorm();
14634
14635 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14636 new KnownBiasAndGravityNormAccelerometerCalibrator(
14637 gravityNorm, measurements,
14638 true, bx, by, bz, sx, sy, sz,
14639 mxy, mxz, myx, myz, mzx, mzy);
14640
14641
14642 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14643 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14644 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14645 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14646 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14647 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14648 final Acceleration bx2 = new Acceleration(0.0,
14649 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14650 calibrator.getBiasXAsAcceleration(bx2);
14651 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14652 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14653 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14654 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14655 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14656 final Acceleration by2 = new Acceleration(0.0,
14657 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14658 calibrator.getBiasYAsAcceleration(by2);
14659 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14660 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14661 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14662 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14663 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14664 final Acceleration bz2 = new Acceleration(0.0,
14665 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14666 calibrator.getBiasZAsAcceleration(bz2);
14667 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14668 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14669 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14670 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14671 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14672 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14673 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14674 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14675 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14676 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14677 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14678 final double[] bias1 = calibrator.getBias();
14679 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14680 final double[] bias2 = new double[3];
14681 calibrator.getBias(bias2);
14682 assertArrayEquals(bias1, bias2, 0.0);
14683 final Matrix b1 = calibrator.getBiasAsMatrix();
14684 assertEquals(b1, ba);
14685 final Matrix b2 = new Matrix(3, 1);
14686 calibrator.getBiasAsMatrix(b2);
14687 assertEquals(b1, b2);
14688 final Matrix ma1 = new Matrix(3, 3);
14689 ma1.setSubmatrix(0, 0,
14690 2, 2,
14691 new double[]{sx, myx, mzx,
14692 mxy, sy, mzy,
14693 mxz, myz, sz});
14694 assertEquals(calibrator.getInitialMa(), ma1);
14695 final Matrix ma2 = new Matrix(3, 3);
14696 calibrator.getInitialMa(ma2);
14697 assertEquals(ma1, ma2);
14698 assertSame(calibrator.getMeasurements(), measurements);
14699 assertTrue(calibrator.isCommonAxisUsed());
14700 assertNull(calibrator.getListener());
14701 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14702 assertFalse(calibrator.isReady());
14703 assertFalse(calibrator.isRunning());
14704 assertNull(calibrator.getEstimatedMa());
14705 assertNull(calibrator.getEstimatedSx());
14706 assertNull(calibrator.getEstimatedSy());
14707 assertNull(calibrator.getEstimatedSz());
14708 assertNull(calibrator.getEstimatedMxy());
14709 assertNull(calibrator.getEstimatedMxz());
14710 assertNull(calibrator.getEstimatedMyx());
14711 assertNull(calibrator.getEstimatedMyz());
14712 assertNull(calibrator.getEstimatedMzx());
14713 assertNull(calibrator.getEstimatedMzy());
14714 assertNull(calibrator.getEstimatedCovariance());
14715 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14716 assertNotNull(calibrator.getGroundTruthGravityNorm());
14717 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14718 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14719 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14720 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14721 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14722 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14723 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14724
14725
14726 calibrator = null;
14727 try {
14728 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14729 -gravityNorm, measurements,
14730 true, bx, by, bz, sx, sy, sz,
14731 mxy, mxz, myx, myz, mzx, mzy);
14732 fail("IllegalArgumentException expected but not thrown");
14733 } catch (final IllegalArgumentException ignore) {
14734 }
14735 assertNull(calibrator);
14736 }
14737
14738 @Test
14739 public void testConstructor132() throws WrongSizeException {
14740 final Collection<StandardDeviationBodyKinematics> measurements =
14741 Collections.emptyList();
14742
14743 final Matrix ba = generateBa();
14744 final double biasX = ba.getElementAtIndex(0);
14745 final double biasY = ba.getElementAtIndex(1);
14746 final double biasZ = ba.getElementAtIndex(2);
14747
14748 final Acceleration bx = new Acceleration(biasX,
14749 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14750 final Acceleration by = new Acceleration(biasY,
14751 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14752 final Acceleration bz = new Acceleration(biasZ,
14753 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14754
14755 final Matrix ma = generateMaCommonAxis();
14756 final double sx = ma.getElementAt(0, 0);
14757 final double sy = ma.getElementAt(1, 1);
14758 final double sz = ma.getElementAt(2, 2);
14759 final double mxy = ma.getElementAt(0, 1);
14760 final double mxz = ma.getElementAt(0, 2);
14761 final double myx = ma.getElementAt(1, 0);
14762 final double myz = ma.getElementAt(1, 2);
14763 final double mzx = ma.getElementAt(2, 0);
14764 final double mzy = ma.getElementAt(2, 1);
14765
14766 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14767 final double latitude = Math.toRadians(
14768 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14769 final double longitude = Math.toRadians(
14770 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14771 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14772 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14773 final NEDVelocity nedVelocity = new NEDVelocity();
14774 final ECEFPosition ecefPosition = new ECEFPosition();
14775 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14776 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14777 ecefPosition, ecefVelocity);
14778 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14779 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14780 final double gravityNorm = gravity.getNorm();
14781
14782 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14783 new KnownBiasAndGravityNormAccelerometerCalibrator(
14784 gravityNorm, measurements,
14785 true, bx, by, bz, sx, sy, sz,
14786 mxy, mxz, myx, myz, mzx, mzy, this);
14787
14788
14789 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14790 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14791 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14792 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14793 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14794 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14795 final Acceleration bx2 = new Acceleration(0.0,
14796 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14797 calibrator.getBiasXAsAcceleration(bx2);
14798 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14799 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14800 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14801 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14802 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14803 final Acceleration by2 = new Acceleration(0.0,
14804 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14805 calibrator.getBiasYAsAcceleration(by2);
14806 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14807 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14808 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14809 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14810 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14811 final Acceleration bz2 = new Acceleration(0.0,
14812 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14813 calibrator.getBiasZAsAcceleration(bz2);
14814 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14815 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14816 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14817 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14818 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14819 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14820 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14821 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14822 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14823 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14824 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14825 final double[] bias1 = calibrator.getBias();
14826 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14827 final double[] bias2 = new double[3];
14828 calibrator.getBias(bias2);
14829 assertArrayEquals(bias1, bias2, 0.0);
14830 final Matrix b1 = calibrator.getBiasAsMatrix();
14831 assertEquals(b1, ba);
14832 final Matrix b2 = new Matrix(3, 1);
14833 calibrator.getBiasAsMatrix(b2);
14834 assertEquals(b1, b2);
14835 final Matrix ma1 = new Matrix(3, 3);
14836 ma1.setSubmatrix(0, 0,
14837 2, 2,
14838 new double[]{sx, myx, mzx,
14839 mxy, sy, mzy,
14840 mxz, myz, sz});
14841 assertEquals(calibrator.getInitialMa(), ma1);
14842 final Matrix ma2 = new Matrix(3, 3);
14843 calibrator.getInitialMa(ma2);
14844 assertEquals(ma1, ma2);
14845 assertSame(calibrator.getMeasurements(), measurements);
14846 assertTrue(calibrator.isCommonAxisUsed());
14847 assertSame(calibrator.getListener(), this);
14848 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14849 assertFalse(calibrator.isReady());
14850 assertFalse(calibrator.isRunning());
14851 assertNull(calibrator.getEstimatedMa());
14852 assertNull(calibrator.getEstimatedSx());
14853 assertNull(calibrator.getEstimatedSy());
14854 assertNull(calibrator.getEstimatedSz());
14855 assertNull(calibrator.getEstimatedMxy());
14856 assertNull(calibrator.getEstimatedMxz());
14857 assertNull(calibrator.getEstimatedMyx());
14858 assertNull(calibrator.getEstimatedMyz());
14859 assertNull(calibrator.getEstimatedMzx());
14860 assertNull(calibrator.getEstimatedMzy());
14861 assertNull(calibrator.getEstimatedCovariance());
14862 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14863 assertNotNull(calibrator.getGroundTruthGravityNorm());
14864 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14865 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14866 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14867 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14868 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14869 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14870 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14871
14872
14873 calibrator = null;
14874 try {
14875 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14876 -gravityNorm, measurements,
14877 true, bx, by, bz, sx, sy, sz,
14878 mxy, mxz, myx, myz, mzx, mzy, this);
14879 fail("IllegalArgumentException expected but not thrown");
14880 } catch (final IllegalArgumentException ignore) {
14881 }
14882 assertNull(calibrator);
14883 }
14884
14885 @Test
14886 public void testConstructor133() throws WrongSizeException {
14887 final Matrix ba = generateBa();
14888 final double[] bias = ba.getBuffer();
14889 final double biasX = ba.getElementAtIndex(0);
14890 final double biasY = ba.getElementAtIndex(1);
14891 final double biasZ = ba.getElementAtIndex(2);
14892
14893 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14894 final double latitude = Math.toRadians(
14895 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14896 final double longitude = Math.toRadians(
14897 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14898 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14899 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14900 final NEDVelocity nedVelocity = new NEDVelocity();
14901 final ECEFPosition ecefPosition = new ECEFPosition();
14902 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14903 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14904 ecefPosition, ecefVelocity);
14905 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14906 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14907 final double gravityNorm = gravity.getNorm();
14908
14909 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14910 new KnownBiasAndGravityNormAccelerometerCalibrator(
14911 gravityNorm, bias);
14912
14913
14914 assertEquals(calibrator.getBiasX(), biasX, 0.0);
14915 assertEquals(calibrator.getBiasY(), biasY, 0.0);
14916 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14917 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14918 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14919 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14920 final Acceleration bx2 = new Acceleration(0.0,
14921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14922 calibrator.getBiasXAsAcceleration(bx2);
14923 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14924 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14925 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14926 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14927 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14928 final Acceleration by2 = new Acceleration(0.0,
14929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14930 calibrator.getBiasYAsAcceleration(by2);
14931 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14932 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14933 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14934 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14935 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14936 final Acceleration bz2 = new Acceleration(0.0,
14937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14938 calibrator.getBiasZAsAcceleration(bz2);
14939 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14940 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14941 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
14942 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
14943 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
14944 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14945 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14946 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14947 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14948 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14949 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14950 final double[] bias1 = calibrator.getBias();
14951 assertArrayEquals(bias1, bias, 0.0);
14952 final double[] bias2 = new double[3];
14953 calibrator.getBias(bias2);
14954 assertArrayEquals(bias1, bias2, 0.0);
14955 final Matrix b1 = calibrator.getBiasAsMatrix();
14956 assertEquals(b1, ba);
14957 final Matrix b2 = new Matrix(3, 1);
14958 calibrator.getBiasAsMatrix(b2);
14959 assertEquals(b1, b2);
14960 final Matrix ma1 = calibrator.getInitialMa();
14961 assertEquals(ma1, new Matrix(3, 3));
14962 final Matrix ma2 = new Matrix(3, 3);
14963 calibrator.getInitialMa(ma2);
14964 assertEquals(ma1, ma2);
14965 assertNull(calibrator.getMeasurements());
14966 assertFalse(calibrator.isCommonAxisUsed());
14967 assertNull(calibrator.getListener());
14968 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14969 assertFalse(calibrator.isReady());
14970 assertFalse(calibrator.isRunning());
14971 assertNull(calibrator.getEstimatedMa());
14972 assertNull(calibrator.getEstimatedSx());
14973 assertNull(calibrator.getEstimatedSy());
14974 assertNull(calibrator.getEstimatedSz());
14975 assertNull(calibrator.getEstimatedMxy());
14976 assertNull(calibrator.getEstimatedMxz());
14977 assertNull(calibrator.getEstimatedMyx());
14978 assertNull(calibrator.getEstimatedMyz());
14979 assertNull(calibrator.getEstimatedMzx());
14980 assertNull(calibrator.getEstimatedMzy());
14981 assertNull(calibrator.getEstimatedCovariance());
14982 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14983 assertNotNull(calibrator.getGroundTruthGravityNorm());
14984 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14985 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14986 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14987 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14988 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14989 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14990 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14991
14992
14993 calibrator = null;
14994 try {
14995 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14996 -gravityNorm, bias);
14997 fail("IllegalArgumentException expected but not thrown");
14998 } catch (final IllegalArgumentException ignore) {
14999 }
15000 try {
15001 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15002 new double[1]);
15003 fail("IllegalArgumentException expected but not thrown");
15004 } catch (final IllegalArgumentException ignore) {
15005 }
15006 assertNull(calibrator);
15007 }
15008
15009 @Test
15010 public void testConstructor134() throws WrongSizeException {
15011 final Matrix ba = generateBa();
15012 final double[] bias = ba.getBuffer();
15013 final double biasX = ba.getElementAtIndex(0);
15014 final double biasY = ba.getElementAtIndex(1);
15015 final double biasZ = ba.getElementAtIndex(2);
15016
15017 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15018 final double latitude = Math.toRadians(
15019 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15020 final double longitude = Math.toRadians(
15021 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15022 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15023 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15024 final NEDVelocity nedVelocity = new NEDVelocity();
15025 final ECEFPosition ecefPosition = new ECEFPosition();
15026 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15027 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15028 ecefPosition, ecefVelocity);
15029 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15030 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15031 final double gravityNorm = gravity.getNorm();
15032
15033 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15034 new KnownBiasAndGravityNormAccelerometerCalibrator(
15035 gravityNorm, bias, this);
15036
15037
15038 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15039 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15040 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15041 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15042 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15043 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15044 final Acceleration bx2 = new Acceleration(0.0,
15045 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15046 calibrator.getBiasXAsAcceleration(bx2);
15047 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15048 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15049 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15050 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15051 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15052 final Acceleration by2 = new Acceleration(0.0,
15053 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15054 calibrator.getBiasYAsAcceleration(by2);
15055 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15056 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15057 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15058 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15059 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15060 final Acceleration bz2 = new Acceleration(0.0,
15061 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15062 calibrator.getBiasZAsAcceleration(bz2);
15063 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15064 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15065 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15066 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15067 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15068 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15069 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15070 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15071 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15072 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15073 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15074 final double[] bias1 = calibrator.getBias();
15075 assertArrayEquals(bias1, bias, 0.0);
15076 final double[] bias2 = new double[3];
15077 calibrator.getBias(bias2);
15078 assertArrayEquals(bias1, bias2, 0.0);
15079 final Matrix b1 = calibrator.getBiasAsMatrix();
15080 assertEquals(b1, ba);
15081 final Matrix b2 = new Matrix(3, 1);
15082 calibrator.getBiasAsMatrix(b2);
15083 assertEquals(b1, b2);
15084 final Matrix ma1 = calibrator.getInitialMa();
15085 assertEquals(ma1, new Matrix(3, 3));
15086 final Matrix ma2 = new Matrix(3, 3);
15087 calibrator.getInitialMa(ma2);
15088 assertEquals(ma1, ma2);
15089 assertNull(calibrator.getMeasurements());
15090 assertFalse(calibrator.isCommonAxisUsed());
15091 assertSame(calibrator.getListener(), this);
15092 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15093 assertFalse(calibrator.isReady());
15094 assertFalse(calibrator.isRunning());
15095 assertNull(calibrator.getEstimatedMa());
15096 assertNull(calibrator.getEstimatedSx());
15097 assertNull(calibrator.getEstimatedSy());
15098 assertNull(calibrator.getEstimatedSz());
15099 assertNull(calibrator.getEstimatedMxy());
15100 assertNull(calibrator.getEstimatedMxz());
15101 assertNull(calibrator.getEstimatedMyx());
15102 assertNull(calibrator.getEstimatedMyz());
15103 assertNull(calibrator.getEstimatedMzx());
15104 assertNull(calibrator.getEstimatedMzy());
15105 assertNull(calibrator.getEstimatedCovariance());
15106 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15107 assertNotNull(calibrator.getGroundTruthGravityNorm());
15108 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15109 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15110 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15111 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15112 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15113 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15114 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15115
15116
15117 calibrator = null;
15118 try {
15119 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15120 -gravityNorm, bias, this);
15121 fail("IllegalArgumentException expected but not thrown");
15122 } catch (final IllegalArgumentException ignore) {
15123 }
15124 try {
15125 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15126 gravityNorm, new double[1], this);
15127 fail("IllegalArgumentException expected but not thrown");
15128 } catch (final IllegalArgumentException ignore) {
15129 }
15130 assertNull(calibrator);
15131 }
15132
15133 @Test
15134 public void testConstructor135() throws WrongSizeException {
15135 final Collection<StandardDeviationBodyKinematics> measurements =
15136 Collections.emptyList();
15137
15138 final Matrix ba = generateBa();
15139 final double[] bias = ba.getBuffer();
15140 final double biasX = ba.getElementAtIndex(0);
15141 final double biasY = ba.getElementAtIndex(1);
15142 final double biasZ = ba.getElementAtIndex(2);
15143
15144 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15145 final double latitude = Math.toRadians(
15146 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15147 final double longitude = Math.toRadians(
15148 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15149 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15150 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15151 final NEDVelocity nedVelocity = new NEDVelocity();
15152 final ECEFPosition ecefPosition = new ECEFPosition();
15153 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15154 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15155 ecefPosition, ecefVelocity);
15156 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15157 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15158 final double gravityNorm = gravity.getNorm();
15159
15160 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15161 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15162 measurements, bias);
15163
15164
15165 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15166 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15167 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15168 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15169 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15170 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15171 final Acceleration bx2 = new Acceleration(0.0,
15172 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15173 calibrator.getBiasXAsAcceleration(bx2);
15174 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15175 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15176 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15177 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15178 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15179 final Acceleration by2 = new Acceleration(0.0,
15180 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15181 calibrator.getBiasYAsAcceleration(by2);
15182 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15183 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15184 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15185 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15186 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15187 final Acceleration bz2 = new Acceleration(0.0,
15188 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15189 calibrator.getBiasZAsAcceleration(bz2);
15190 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15191 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15192 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15193 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15194 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15195 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15196 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15197 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15198 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15199 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15200 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15201 final double[] bias1 = calibrator.getBias();
15202 assertArrayEquals(bias1, bias, 0.0);
15203 final double[] bias2 = new double[3];
15204 calibrator.getBias(bias2);
15205 assertArrayEquals(bias1, bias2, 0.0);
15206 final Matrix b1 = calibrator.getBiasAsMatrix();
15207 assertEquals(b1, ba);
15208 final Matrix b2 = new Matrix(3, 1);
15209 calibrator.getBiasAsMatrix(b2);
15210 assertEquals(b1, b2);
15211 final Matrix ma1 = calibrator.getInitialMa();
15212 assertEquals(ma1, new Matrix(3, 3));
15213 final Matrix ma2 = new Matrix(3, 3);
15214 calibrator.getInitialMa(ma2);
15215 assertEquals(ma1, ma2);
15216 assertSame(calibrator.getMeasurements(), measurements);
15217 assertFalse(calibrator.isCommonAxisUsed());
15218 assertNull(calibrator.getListener());
15219 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15220 assertFalse(calibrator.isReady());
15221 assertFalse(calibrator.isRunning());
15222 assertNull(calibrator.getEstimatedMa());
15223 assertNull(calibrator.getEstimatedSx());
15224 assertNull(calibrator.getEstimatedSy());
15225 assertNull(calibrator.getEstimatedSz());
15226 assertNull(calibrator.getEstimatedMxy());
15227 assertNull(calibrator.getEstimatedMxz());
15228 assertNull(calibrator.getEstimatedMyx());
15229 assertNull(calibrator.getEstimatedMyz());
15230 assertNull(calibrator.getEstimatedMzx());
15231 assertNull(calibrator.getEstimatedMzy());
15232 assertNull(calibrator.getEstimatedCovariance());
15233 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15234 assertNotNull(calibrator.getGroundTruthGravityNorm());
15235 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15236 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15237 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15238 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15239 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15240 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15241 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15242
15243
15244 calibrator = null;
15245 try {
15246 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15247 -gravityNorm, measurements, bias);
15248 fail("IllegalArgumentException expected but not thrown");
15249 } catch (final IllegalArgumentException ignore) {
15250 }
15251 try {
15252 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15253 gravityNorm, measurements, new double[1]);
15254 fail("IllegalArgumentException expected but not thrown");
15255 } catch (final IllegalArgumentException ignore) {
15256 }
15257 assertNull(calibrator);
15258 }
15259
15260 @Test
15261 public void testConstructor136() throws WrongSizeException {
15262 final Collection<StandardDeviationBodyKinematics> measurements =
15263 Collections.emptyList();
15264
15265 final Matrix ba = generateBa();
15266 final double[] bias = ba.getBuffer();
15267 final double biasX = ba.getElementAtIndex(0);
15268 final double biasY = ba.getElementAtIndex(1);
15269 final double biasZ = ba.getElementAtIndex(2);
15270
15271 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15272 final double latitude = Math.toRadians(
15273 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15274 final double longitude = Math.toRadians(
15275 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15276 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15277 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15278 final NEDVelocity nedVelocity = new NEDVelocity();
15279 final ECEFPosition ecefPosition = new ECEFPosition();
15280 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15281 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15282 ecefPosition, ecefVelocity);
15283 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15284 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15285 final double gravityNorm = gravity.getNorm();
15286
15287 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15288 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15289 measurements, bias, this);
15290
15291
15292 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15293 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15294 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15295 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15296 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15297 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15298 final Acceleration bx2 = new Acceleration(0.0,
15299 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15300 calibrator.getBiasXAsAcceleration(bx2);
15301 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15302 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15303 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15304 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15305 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15306 final Acceleration by2 = new Acceleration(0.0,
15307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15308 calibrator.getBiasYAsAcceleration(by2);
15309 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15310 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15311 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15312 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15313 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15314 final Acceleration bz2 = new Acceleration(0.0,
15315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15316 calibrator.getBiasZAsAcceleration(bz2);
15317 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15318 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15319 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15320 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15321 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15322 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15323 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15324 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15325 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15326 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15327 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15328 final double[] bias1 = calibrator.getBias();
15329 assertArrayEquals(bias1, bias, 0.0);
15330 final double[] bias2 = new double[3];
15331 calibrator.getBias(bias2);
15332 assertArrayEquals(bias1, bias2, 0.0);
15333 final Matrix b1 = calibrator.getBiasAsMatrix();
15334 assertEquals(b1, ba);
15335 final Matrix b2 = new Matrix(3, 1);
15336 calibrator.getBiasAsMatrix(b2);
15337 assertEquals(b1, b2);
15338 final Matrix ma1 = calibrator.getInitialMa();
15339 assertEquals(ma1, new Matrix(3, 3));
15340 final Matrix ma2 = new Matrix(3, 3);
15341 calibrator.getInitialMa(ma2);
15342 assertEquals(ma1, ma2);
15343 assertSame(calibrator.getMeasurements(), measurements);
15344 assertFalse(calibrator.isCommonAxisUsed());
15345 assertSame(calibrator.getListener(), this);
15346 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15347 assertFalse(calibrator.isReady());
15348 assertFalse(calibrator.isRunning());
15349 assertNull(calibrator.getEstimatedMa());
15350 assertNull(calibrator.getEstimatedSx());
15351 assertNull(calibrator.getEstimatedSy());
15352 assertNull(calibrator.getEstimatedSz());
15353 assertNull(calibrator.getEstimatedMxy());
15354 assertNull(calibrator.getEstimatedMxz());
15355 assertNull(calibrator.getEstimatedMyx());
15356 assertNull(calibrator.getEstimatedMyz());
15357 assertNull(calibrator.getEstimatedMzx());
15358 assertNull(calibrator.getEstimatedMzy());
15359 assertNull(calibrator.getEstimatedCovariance());
15360 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15361 assertNotNull(calibrator.getGroundTruthGravityNorm());
15362 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15363 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15364 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15365 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15366 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15367 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15368 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15369
15370
15371 calibrator = null;
15372 try {
15373 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15374 -gravityNorm, measurements, bias, this);
15375 fail("IllegalArgumentException expected but not thrown");
15376 } catch (final IllegalArgumentException ignore) {
15377 }
15378 try {
15379 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15380 gravityNorm, measurements, new double[1], this);
15381 fail("IllegalArgumentException expected but not thrown");
15382 } catch (final IllegalArgumentException ignore) {
15383 }
15384 assertNull(calibrator);
15385 }
15386
15387 @Test
15388 public void testConstructor137() throws WrongSizeException {
15389 final Matrix ba = generateBa();
15390 final double[] bias = ba.getBuffer();
15391 final double biasX = ba.getElementAtIndex(0);
15392 final double biasY = ba.getElementAtIndex(1);
15393 final double biasZ = ba.getElementAtIndex(2);
15394
15395 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15396 final double latitude = Math.toRadians(
15397 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15398 final double longitude = Math.toRadians(
15399 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15400 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15401 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15402 final NEDVelocity nedVelocity = new NEDVelocity();
15403 final ECEFPosition ecefPosition = new ECEFPosition();
15404 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15405 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15406 ecefPosition, ecefVelocity);
15407 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15408 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15409 final double gravityNorm = gravity.getNorm();
15410
15411 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15412 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15413 true, bias);
15414
15415
15416 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15417 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15418 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15419 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15420 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15421 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15422 final Acceleration bx2 = new Acceleration(0.0,
15423 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15424 calibrator.getBiasXAsAcceleration(bx2);
15425 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15426 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15427 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15428 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15429 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15430 final Acceleration by2 = new Acceleration(0.0,
15431 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15432 calibrator.getBiasYAsAcceleration(by2);
15433 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15434 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15435 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15436 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15437 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15438 final Acceleration bz2 = new Acceleration(0.0,
15439 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15440 calibrator.getBiasZAsAcceleration(bz2);
15441 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15442 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15443 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15444 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15445 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15446 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15447 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15448 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15449 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15450 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15451 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15452 final double[] bias1 = calibrator.getBias();
15453 assertArrayEquals(bias1, bias, 0.0);
15454 final double[] bias2 = new double[3];
15455 calibrator.getBias(bias2);
15456 assertArrayEquals(bias1, bias2, 0.0);
15457 final Matrix b1 = calibrator.getBiasAsMatrix();
15458 assertEquals(b1, ba);
15459 final Matrix b2 = new Matrix(3, 1);
15460 calibrator.getBiasAsMatrix(b2);
15461 assertEquals(b1, b2);
15462 final Matrix ma1 = calibrator.getInitialMa();
15463 assertEquals(ma1, new Matrix(3, 3));
15464 final Matrix ma2 = new Matrix(3, 3);
15465 calibrator.getInitialMa(ma2);
15466 assertEquals(ma1, ma2);
15467 assertNull(calibrator.getMeasurements());
15468 assertTrue(calibrator.isCommonAxisUsed());
15469 assertNull(calibrator.getListener());
15470 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15471 assertFalse(calibrator.isReady());
15472 assertFalse(calibrator.isRunning());
15473 assertNull(calibrator.getEstimatedMa());
15474 assertNull(calibrator.getEstimatedSx());
15475 assertNull(calibrator.getEstimatedSy());
15476 assertNull(calibrator.getEstimatedSz());
15477 assertNull(calibrator.getEstimatedMxy());
15478 assertNull(calibrator.getEstimatedMxz());
15479 assertNull(calibrator.getEstimatedMyx());
15480 assertNull(calibrator.getEstimatedMyz());
15481 assertNull(calibrator.getEstimatedMzx());
15482 assertNull(calibrator.getEstimatedMzy());
15483 assertNull(calibrator.getEstimatedCovariance());
15484 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15485 assertNotNull(calibrator.getGroundTruthGravityNorm());
15486 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15487 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15488 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15489 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15490 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15491 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15492 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15493
15494
15495 calibrator = null;
15496 try {
15497 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15498 -gravityNorm, true, bias);
15499 fail("IllegalArgumentException expected but not thrown");
15500 } catch (final IllegalArgumentException ignore) {
15501 }
15502 try {
15503 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15504 gravityNorm, true, new double[1]);
15505 fail("IllegalArgumentException expected but not thrown");
15506 } catch (final IllegalArgumentException ignore) {
15507 }
15508 assertNull(calibrator);
15509 }
15510
15511 @Test
15512 public void testConstructor138() throws WrongSizeException {
15513 final Matrix ba = generateBa();
15514 final double[] bias = ba.getBuffer();
15515 final double biasX = ba.getElementAtIndex(0);
15516 final double biasY = ba.getElementAtIndex(1);
15517 final double biasZ = ba.getElementAtIndex(2);
15518
15519 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15520 final double latitude = Math.toRadians(
15521 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15522 final double longitude = Math.toRadians(
15523 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15524 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15525 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15526 final NEDVelocity nedVelocity = new NEDVelocity();
15527 final ECEFPosition ecefPosition = new ECEFPosition();
15528 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15529 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15530 ecefPosition, ecefVelocity);
15531 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15532 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15533 final double gravityNorm = gravity.getNorm();
15534
15535 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15536 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15537 true, bias, this);
15538
15539
15540 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15541 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15542 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15543 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15544 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15545 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15546 final Acceleration bx2 = new Acceleration(0.0,
15547 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15548 calibrator.getBiasXAsAcceleration(bx2);
15549 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15550 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15551 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15552 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15553 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15554 final Acceleration by2 = new Acceleration(0.0,
15555 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15556 calibrator.getBiasYAsAcceleration(by2);
15557 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15558 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15559 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15560 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15561 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15562 final Acceleration bz2 = new Acceleration(0.0,
15563 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15564 calibrator.getBiasZAsAcceleration(bz2);
15565 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15566 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15567 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15568 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15569 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15570 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15571 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15572 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15573 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15574 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15575 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15576 final double[] bias1 = calibrator.getBias();
15577 assertArrayEquals(bias1, bias, 0.0);
15578 final double[] bias2 = new double[3];
15579 calibrator.getBias(bias2);
15580 assertArrayEquals(bias1, bias2, 0.0);
15581 final Matrix b1 = calibrator.getBiasAsMatrix();
15582 assertEquals(b1, ba);
15583 final Matrix b2 = new Matrix(3, 1);
15584 calibrator.getBiasAsMatrix(b2);
15585 assertEquals(b1, b2);
15586 final Matrix ma1 = calibrator.getInitialMa();
15587 assertEquals(ma1, new Matrix(3, 3));
15588 final Matrix ma2 = new Matrix(3, 3);
15589 calibrator.getInitialMa(ma2);
15590 assertEquals(ma1, ma2);
15591 assertNull(calibrator.getMeasurements());
15592 assertTrue(calibrator.isCommonAxisUsed());
15593 assertSame(calibrator.getListener(), this);
15594 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15595 assertFalse(calibrator.isReady());
15596 assertFalse(calibrator.isRunning());
15597 assertNull(calibrator.getEstimatedMa());
15598 assertNull(calibrator.getEstimatedSx());
15599 assertNull(calibrator.getEstimatedSy());
15600 assertNull(calibrator.getEstimatedSz());
15601 assertNull(calibrator.getEstimatedMxy());
15602 assertNull(calibrator.getEstimatedMxz());
15603 assertNull(calibrator.getEstimatedMyx());
15604 assertNull(calibrator.getEstimatedMyz());
15605 assertNull(calibrator.getEstimatedMzx());
15606 assertNull(calibrator.getEstimatedMzy());
15607 assertNull(calibrator.getEstimatedCovariance());
15608 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15609 assertNotNull(calibrator.getGroundTruthGravityNorm());
15610 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15611 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15612 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15613 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15614 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15615 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15616 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15617
15618
15619 calibrator = null;
15620 try {
15621 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15622 -gravityNorm, true, bias, this);
15623 fail("IllegalArgumentException expected but not thrown");
15624 } catch (final IllegalArgumentException ignore) {
15625 }
15626 try {
15627 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15628 gravityNorm, true, new double[1],
15629 this);
15630 fail("IllegalArgumentException expected but not thrown");
15631 } catch (final IllegalArgumentException ignore) {
15632 }
15633 assertNull(calibrator);
15634 }
15635
15636 @Test
15637 public void testConstructor139() throws WrongSizeException {
15638 final Collection<StandardDeviationBodyKinematics> measurements =
15639 Collections.emptyList();
15640
15641 final Matrix ba = generateBa();
15642 final double[] bias = ba.getBuffer();
15643 final double biasX = ba.getElementAtIndex(0);
15644 final double biasY = ba.getElementAtIndex(1);
15645 final double biasZ = ba.getElementAtIndex(2);
15646
15647 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15648 final double latitude = Math.toRadians(
15649 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15650 final double longitude = Math.toRadians(
15651 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15652 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15653 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15654 final NEDVelocity nedVelocity = new NEDVelocity();
15655 final ECEFPosition ecefPosition = new ECEFPosition();
15656 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15657 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15658 ecefPosition, ecefVelocity);
15659 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15660 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15661 final double gravityNorm = gravity.getNorm();
15662
15663 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15664 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15665 measurements, true, bias);
15666
15667
15668 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15669 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15670 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15671 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15672 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15673 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15674 final Acceleration bx2 = new Acceleration(0.0,
15675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15676 calibrator.getBiasXAsAcceleration(bx2);
15677 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15678 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15679 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15680 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15681 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15682 final Acceleration by2 = new Acceleration(0.0,
15683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15684 calibrator.getBiasYAsAcceleration(by2);
15685 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15686 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15687 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15688 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15689 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15690 final Acceleration bz2 = new Acceleration(0.0,
15691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15692 calibrator.getBiasZAsAcceleration(bz2);
15693 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15694 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15695 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15696 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15697 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15698 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15699 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15700 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15701 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15702 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15703 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15704 final double[] bias1 = calibrator.getBias();
15705 assertArrayEquals(bias1, bias, 0.0);
15706 final double[] bias2 = new double[3];
15707 calibrator.getBias(bias2);
15708 assertArrayEquals(bias1, bias2, 0.0);
15709 final Matrix b1 = calibrator.getBiasAsMatrix();
15710 assertEquals(b1, ba);
15711 final Matrix b2 = new Matrix(3, 1);
15712 calibrator.getBiasAsMatrix(b2);
15713 assertEquals(b1, b2);
15714 final Matrix ma1 = calibrator.getInitialMa();
15715 assertEquals(ma1, new Matrix(3, 3));
15716 final Matrix ma2 = new Matrix(3, 3);
15717 calibrator.getInitialMa(ma2);
15718 assertEquals(ma1, ma2);
15719 assertSame(calibrator.getMeasurements(), measurements);
15720 assertTrue(calibrator.isCommonAxisUsed());
15721 assertNull(calibrator.getListener());
15722 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15723 assertFalse(calibrator.isReady());
15724 assertFalse(calibrator.isRunning());
15725 assertNull(calibrator.getEstimatedMa());
15726 assertNull(calibrator.getEstimatedSx());
15727 assertNull(calibrator.getEstimatedSy());
15728 assertNull(calibrator.getEstimatedSz());
15729 assertNull(calibrator.getEstimatedMxy());
15730 assertNull(calibrator.getEstimatedMxz());
15731 assertNull(calibrator.getEstimatedMyx());
15732 assertNull(calibrator.getEstimatedMyz());
15733 assertNull(calibrator.getEstimatedMzx());
15734 assertNull(calibrator.getEstimatedMzy());
15735 assertNull(calibrator.getEstimatedCovariance());
15736 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15737 assertNotNull(calibrator.getGroundTruthGravityNorm());
15738 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15739 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15740 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15741 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15742 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15743 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15744 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15745
15746
15747 calibrator = null;
15748 try {
15749 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15750 -gravityNorm, measurements, true, bias);
15751 fail("IllegalArgumentException expected but not thrown");
15752 } catch (final IllegalArgumentException ignore) {
15753 }
15754 try {
15755 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15756 gravityNorm, measurements, true,
15757 new double[1]);
15758 fail("IllegalArgumentException expected but not thrown");
15759 } catch (final IllegalArgumentException ignore) {
15760 }
15761 assertNull(calibrator);
15762 }
15763
15764 @Test
15765 public void testConstructor140() throws WrongSizeException {
15766 final Collection<StandardDeviationBodyKinematics> measurements =
15767 Collections.emptyList();
15768
15769 final Matrix ba = generateBa();
15770 final double[] bias = ba.getBuffer();
15771 final double biasX = ba.getElementAtIndex(0);
15772 final double biasY = ba.getElementAtIndex(1);
15773 final double biasZ = ba.getElementAtIndex(2);
15774
15775 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15776 final double latitude = Math.toRadians(
15777 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15778 final double longitude = Math.toRadians(
15779 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15780 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15781 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15782 final NEDVelocity nedVelocity = new NEDVelocity();
15783 final ECEFPosition ecefPosition = new ECEFPosition();
15784 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15785 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15786 ecefPosition, ecefVelocity);
15787 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15788 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15789 final double gravityNorm = gravity.getNorm();
15790
15791 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15792 new KnownBiasAndGravityNormAccelerometerCalibrator(
15793 gravityNorm, measurements,
15794 true, bias, this);
15795
15796
15797 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15798 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15799 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15800 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15801 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15802 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15803 final Acceleration bx2 = new Acceleration(0.0,
15804 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15805 calibrator.getBiasXAsAcceleration(bx2);
15806 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15807 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15808 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15809 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15810 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15811 final Acceleration by2 = new Acceleration(0.0,
15812 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15813 calibrator.getBiasYAsAcceleration(by2);
15814 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15815 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15816 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15817 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15818 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15819 final Acceleration bz2 = new Acceleration(0.0,
15820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15821 calibrator.getBiasZAsAcceleration(bz2);
15822 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15823 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15824 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15825 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15826 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15827 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15828 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15829 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15830 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15831 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15832 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15833 final double[] bias1 = calibrator.getBias();
15834 assertArrayEquals(bias1, bias, 0.0);
15835 final double[] bias2 = new double[3];
15836 calibrator.getBias(bias2);
15837 assertArrayEquals(bias1, bias2, 0.0);
15838 final Matrix b1 = calibrator.getBiasAsMatrix();
15839 assertEquals(b1, ba);
15840 final Matrix b2 = new Matrix(3, 1);
15841 calibrator.getBiasAsMatrix(b2);
15842 assertEquals(b1, b2);
15843 final Matrix ma1 = calibrator.getInitialMa();
15844 assertEquals(ma1, new Matrix(3, 3));
15845 final Matrix ma2 = new Matrix(3, 3);
15846 calibrator.getInitialMa(ma2);
15847 assertEquals(ma1, ma2);
15848 assertSame(calibrator.getMeasurements(), measurements);
15849 assertTrue(calibrator.isCommonAxisUsed());
15850 assertSame(calibrator.getListener(), this);
15851 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15852 assertFalse(calibrator.isReady());
15853 assertFalse(calibrator.isRunning());
15854 assertNull(calibrator.getEstimatedMa());
15855 assertNull(calibrator.getEstimatedSx());
15856 assertNull(calibrator.getEstimatedSy());
15857 assertNull(calibrator.getEstimatedSz());
15858 assertNull(calibrator.getEstimatedMxy());
15859 assertNull(calibrator.getEstimatedMxz());
15860 assertNull(calibrator.getEstimatedMyx());
15861 assertNull(calibrator.getEstimatedMyz());
15862 assertNull(calibrator.getEstimatedMzx());
15863 assertNull(calibrator.getEstimatedMzy());
15864 assertNull(calibrator.getEstimatedCovariance());
15865 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15866 assertNotNull(calibrator.getGroundTruthGravityNorm());
15867 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15868 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15869 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15870 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15871 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15872 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15873 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15874
15875
15876 calibrator = null;
15877 try {
15878 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15879 -gravityNorm, measurements,
15880 true, bias, this);
15881 fail("IllegalArgumentException expected but not thrown");
15882 } catch (final IllegalArgumentException ignore) {
15883 }
15884 try {
15885 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15886 gravityNorm, measurements, true,
15887 new double[1], this);
15888 fail("IllegalArgumentException expected but not thrown");
15889 } catch (final IllegalArgumentException ignore) {
15890 }
15891 assertNull(calibrator);
15892 }
15893
15894 @Test
15895 public void testConstructor141() throws WrongSizeException {
15896 final Matrix ba = generateBa();
15897 final double[] bias = ba.getBuffer();
15898 final double biasX = ba.getElementAtIndex(0);
15899 final double biasY = ba.getElementAtIndex(1);
15900 final double biasZ = ba.getElementAtIndex(2);
15901
15902 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15903 final double latitude = Math.toRadians(
15904 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15905 final double longitude = Math.toRadians(
15906 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15907 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15908 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15909 final NEDVelocity nedVelocity = new NEDVelocity();
15910 final ECEFPosition ecefPosition = new ECEFPosition();
15911 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15912 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15913 ecefPosition, ecefVelocity);
15914 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15915 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15916 final double gravityNorm = gravity.getNorm();
15917
15918 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15919 new KnownBiasAndGravityNormAccelerometerCalibrator(
15920 gravityNorm, ba);
15921
15922
15923 assertEquals(calibrator.getBiasX(), biasX, 0.0);
15924 assertEquals(calibrator.getBiasY(), biasY, 0.0);
15925 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15926 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15927 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15928 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15929 final Acceleration bx2 = new Acceleration(0.0,
15930 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15931 calibrator.getBiasXAsAcceleration(bx2);
15932 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15933 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15934 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15935 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15936 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15937 final Acceleration by2 = new Acceleration(0.0,
15938 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15939 calibrator.getBiasYAsAcceleration(by2);
15940 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15941 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15942 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15943 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15944 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15945 final Acceleration bz2 = new Acceleration(0.0,
15946 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15947 calibrator.getBiasZAsAcceleration(bz2);
15948 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15949 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15950 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15951 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15952 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15953 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15954 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15955 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15956 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15957 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15958 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15959 final double[] bias1 = calibrator.getBias();
15960 assertArrayEquals(bias1, bias, 0.0);
15961 final double[] bias2 = new double[3];
15962 calibrator.getBias(bias2);
15963 assertArrayEquals(bias1, bias2, 0.0);
15964 final Matrix b1 = calibrator.getBiasAsMatrix();
15965 assertEquals(b1, ba);
15966 final Matrix b2 = new Matrix(3, 1);
15967 calibrator.getBiasAsMatrix(b2);
15968 assertEquals(b1, b2);
15969 final Matrix ma1 = calibrator.getInitialMa();
15970 assertEquals(ma1, new Matrix(3, 3));
15971 final Matrix ma2 = new Matrix(3, 3);
15972 calibrator.getInitialMa(ma2);
15973 assertEquals(ma1, ma2);
15974 assertNull(calibrator.getMeasurements());
15975 assertFalse(calibrator.isCommonAxisUsed());
15976 assertNull(calibrator.getListener());
15977 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15978 assertFalse(calibrator.isReady());
15979 assertFalse(calibrator.isRunning());
15980 assertNull(calibrator.getEstimatedMa());
15981 assertNull(calibrator.getEstimatedSx());
15982 assertNull(calibrator.getEstimatedSy());
15983 assertNull(calibrator.getEstimatedSz());
15984 assertNull(calibrator.getEstimatedMxy());
15985 assertNull(calibrator.getEstimatedMxz());
15986 assertNull(calibrator.getEstimatedMyx());
15987 assertNull(calibrator.getEstimatedMyz());
15988 assertNull(calibrator.getEstimatedMzx());
15989 assertNull(calibrator.getEstimatedMzy());
15990 assertNull(calibrator.getEstimatedCovariance());
15991 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15992 assertNotNull(calibrator.getGroundTruthGravityNorm());
15993 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15994 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15995 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15996 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15997 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15998 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15999 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16000
16001
16002 calibrator = null;
16003 try {
16004 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16005 -gravityNorm, ba);
16006 fail("IllegalArgumentException expected but not thrown");
16007 } catch (final IllegalArgumentException ignore) {
16008 }
16009 try {
16010 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16011 gravityNorm, new Matrix(1, 1));
16012 fail("IllegalArgumentException expected but not thrown");
16013 } catch (final IllegalArgumentException ignore) {
16014 }
16015 try {
16016 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16017 gravityNorm, new Matrix(1, 3));
16018 fail("IllegalArgumentException expected but not thrown");
16019 } catch (final IllegalArgumentException ignore) {
16020 }
16021 assertNull(calibrator);
16022 }
16023
16024 @Test
16025 public void testConstructor142() throws WrongSizeException {
16026 final Matrix ba = generateBa();
16027 final double[] bias = ba.getBuffer();
16028 final double biasX = ba.getElementAtIndex(0);
16029 final double biasY = ba.getElementAtIndex(1);
16030 final double biasZ = ba.getElementAtIndex(2);
16031
16032 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16033 final double latitude = Math.toRadians(
16034 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16035 final double longitude = Math.toRadians(
16036 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16037 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16038 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16039 final NEDVelocity nedVelocity = new NEDVelocity();
16040 final ECEFPosition ecefPosition = new ECEFPosition();
16041 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16042 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16043 ecefPosition, ecefVelocity);
16044 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16045 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16046 final double gravityNorm = gravity.getNorm();
16047
16048 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16049 new KnownBiasAndGravityNormAccelerometerCalibrator(
16050 gravityNorm, ba, this);
16051
16052
16053 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16054 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16055 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16056 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16057 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16058 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16059 final Acceleration bx2 = new Acceleration(0.0,
16060 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16061 calibrator.getBiasXAsAcceleration(bx2);
16062 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16063 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16064 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16065 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16066 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16067 final Acceleration by2 = new Acceleration(0.0,
16068 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16069 calibrator.getBiasYAsAcceleration(by2);
16070 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16071 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16072 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16073 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16074 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16075 final Acceleration bz2 = new Acceleration(0.0,
16076 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16077 calibrator.getBiasZAsAcceleration(bz2);
16078 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16079 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16080 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16081 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16082 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16083 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16084 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16085 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16086 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16087 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16088 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16089 final double[] bias1 = calibrator.getBias();
16090 assertArrayEquals(bias1, bias, 0.0);
16091 final double[] bias2 = new double[3];
16092 calibrator.getBias(bias2);
16093 assertArrayEquals(bias1, bias2, 0.0);
16094 final Matrix b1 = calibrator.getBiasAsMatrix();
16095 assertEquals(b1, ba);
16096 final Matrix b2 = new Matrix(3, 1);
16097 calibrator.getBiasAsMatrix(b2);
16098 assertEquals(b1, b2);
16099 final Matrix ma1 = calibrator.getInitialMa();
16100 assertEquals(ma1, new Matrix(3, 3));
16101 final Matrix ma2 = new Matrix(3, 3);
16102 calibrator.getInitialMa(ma2);
16103 assertEquals(ma1, ma2);
16104 assertNull(calibrator.getMeasurements());
16105 assertFalse(calibrator.isCommonAxisUsed());
16106 assertSame(calibrator.getListener(), this);
16107 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16108 assertFalse(calibrator.isReady());
16109 assertFalse(calibrator.isRunning());
16110 assertNull(calibrator.getEstimatedMa());
16111 assertNull(calibrator.getEstimatedSx());
16112 assertNull(calibrator.getEstimatedSy());
16113 assertNull(calibrator.getEstimatedSz());
16114 assertNull(calibrator.getEstimatedMxy());
16115 assertNull(calibrator.getEstimatedMxz());
16116 assertNull(calibrator.getEstimatedMyx());
16117 assertNull(calibrator.getEstimatedMyz());
16118 assertNull(calibrator.getEstimatedMzx());
16119 assertNull(calibrator.getEstimatedMzy());
16120 assertNull(calibrator.getEstimatedCovariance());
16121 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16122 assertNotNull(calibrator.getGroundTruthGravityNorm());
16123 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16124 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16125 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16126 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16127 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16128 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16129 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16130
16131
16132 calibrator = null;
16133 try {
16134 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16135 -gravityNorm, ba, this);
16136 fail("IllegalArgumentException expected but not thrown");
16137 } catch (final IllegalArgumentException ignore) {
16138 }
16139 try {
16140 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16141 gravityNorm, new Matrix(1, 1), this);
16142 fail("IllegalArgumentException expected but not thrown");
16143 } catch (final IllegalArgumentException ignore) {
16144 }
16145 try {
16146 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16147 gravityNorm, new Matrix(1, 3),
16148 this);
16149 fail("IllegalArgumentException expected but not thrown");
16150 } catch (final IllegalArgumentException ignore) {
16151 }
16152 assertNull(calibrator);
16153 }
16154
16155 @Test
16156 public void testConstructor143() throws WrongSizeException {
16157 final Collection<StandardDeviationBodyKinematics> measurements =
16158 Collections.emptyList();
16159
16160 final Matrix ba = generateBa();
16161 final double[] bias = ba.getBuffer();
16162 final double biasX = ba.getElementAtIndex(0);
16163 final double biasY = ba.getElementAtIndex(1);
16164 final double biasZ = ba.getElementAtIndex(2);
16165
16166 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16167 final double latitude = Math.toRadians(
16168 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16169 final double longitude = Math.toRadians(
16170 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16171 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16172 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16173 final NEDVelocity nedVelocity = new NEDVelocity();
16174 final ECEFPosition ecefPosition = new ECEFPosition();
16175 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16176 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16177 ecefPosition, ecefVelocity);
16178 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16179 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16180 final double gravityNorm = gravity.getNorm();
16181
16182 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16183 new KnownBiasAndGravityNormAccelerometerCalibrator(
16184 gravityNorm, measurements, ba);
16185
16186
16187 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16188 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16189 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16190 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16191 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16192 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16193 final Acceleration bx2 = new Acceleration(0.0,
16194 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16195 calibrator.getBiasXAsAcceleration(bx2);
16196 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16197 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16198 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16199 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16200 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16201 final Acceleration by2 = new Acceleration(0.0,
16202 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16203 calibrator.getBiasYAsAcceleration(by2);
16204 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16205 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16206 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16207 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16208 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16209 final Acceleration bz2 = new Acceleration(0.0,
16210 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16211 calibrator.getBiasZAsAcceleration(bz2);
16212 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16213 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16214 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16215 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16216 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16217 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16218 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16219 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16220 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16221 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16222 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16223 final double[] bias1 = calibrator.getBias();
16224 assertArrayEquals(bias1, bias, 0.0);
16225 final double[] bias2 = new double[3];
16226 calibrator.getBias(bias2);
16227 assertArrayEquals(bias1, bias2, 0.0);
16228 final Matrix b1 = calibrator.getBiasAsMatrix();
16229 assertEquals(b1, ba);
16230 final Matrix b2 = new Matrix(3, 1);
16231 calibrator.getBiasAsMatrix(b2);
16232 assertEquals(b1, b2);
16233 final Matrix ma1 = calibrator.getInitialMa();
16234 assertEquals(ma1, new Matrix(3, 3));
16235 final Matrix ma2 = new Matrix(3, 3);
16236 calibrator.getInitialMa(ma2);
16237 assertEquals(ma1, ma2);
16238 assertSame(calibrator.getMeasurements(), measurements);
16239 assertFalse(calibrator.isCommonAxisUsed());
16240 assertNull(calibrator.getListener());
16241 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16242 assertFalse(calibrator.isReady());
16243 assertFalse(calibrator.isRunning());
16244 assertNull(calibrator.getEstimatedMa());
16245 assertNull(calibrator.getEstimatedSx());
16246 assertNull(calibrator.getEstimatedSy());
16247 assertNull(calibrator.getEstimatedSz());
16248 assertNull(calibrator.getEstimatedMxy());
16249 assertNull(calibrator.getEstimatedMxz());
16250 assertNull(calibrator.getEstimatedMyx());
16251 assertNull(calibrator.getEstimatedMyz());
16252 assertNull(calibrator.getEstimatedMzx());
16253 assertNull(calibrator.getEstimatedMzy());
16254 assertNull(calibrator.getEstimatedCovariance());
16255 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16256 assertNotNull(calibrator.getGroundTruthGravityNorm());
16257 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16258 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16259 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16260 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16261 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16262 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16263 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16264
16265
16266 calibrator = null;
16267 try {
16268 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16269 -gravityNorm, measurements, ba);
16270 fail("IllegalArgumentException expected but not thrown");
16271 } catch (final IllegalArgumentException ignore) {
16272 }
16273 try {
16274 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16275 gravityNorm, measurements,
16276 new Matrix(1, 1));
16277 fail("IllegalArgumentException expected but not thrown");
16278 } catch (final IllegalArgumentException ignore) {
16279 }
16280 try {
16281 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16282 gravityNorm, measurements,
16283 new Matrix(1, 3));
16284 fail("IllegalArgumentException expected but not thrown");
16285 } catch (final IllegalArgumentException ignore) {
16286 }
16287 assertNull(calibrator);
16288 }
16289
16290 @Test
16291 public void testConstructor144() throws WrongSizeException {
16292 final Collection<StandardDeviationBodyKinematics> measurements =
16293 Collections.emptyList();
16294
16295 final Matrix ba = generateBa();
16296 final double[] bias = ba.getBuffer();
16297 final double biasX = ba.getElementAtIndex(0);
16298 final double biasY = ba.getElementAtIndex(1);
16299 final double biasZ = ba.getElementAtIndex(2);
16300
16301 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16302 final double latitude = Math.toRadians(
16303 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16304 final double longitude = Math.toRadians(
16305 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16306 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16307 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16308 final NEDVelocity nedVelocity = new NEDVelocity();
16309 final ECEFPosition ecefPosition = new ECEFPosition();
16310 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16311 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16312 ecefPosition, ecefVelocity);
16313 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16314 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16315 final double gravityNorm = gravity.getNorm();
16316
16317 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16318 new KnownBiasAndGravityNormAccelerometerCalibrator(
16319 gravityNorm, measurements, ba, this);
16320
16321
16322 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16323 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16324 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16325 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16326 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16327 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16328 final Acceleration bx2 = new Acceleration(0.0,
16329 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16330 calibrator.getBiasXAsAcceleration(bx2);
16331 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16332 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16333 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16334 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16335 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16336 final Acceleration by2 = new Acceleration(0.0,
16337 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16338 calibrator.getBiasYAsAcceleration(by2);
16339 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16340 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16341 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16342 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16343 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16344 final Acceleration bz2 = new Acceleration(0.0,
16345 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16346 calibrator.getBiasZAsAcceleration(bz2);
16347 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16348 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16349 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16350 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16351 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16352 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16353 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16354 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16355 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16356 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16357 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16358 final double[] bias1 = calibrator.getBias();
16359 assertArrayEquals(bias1, bias, 0.0);
16360 final double[] bias2 = new double[3];
16361 calibrator.getBias(bias2);
16362 assertArrayEquals(bias1, bias2, 0.0);
16363 final Matrix b1 = calibrator.getBiasAsMatrix();
16364 assertEquals(b1, ba);
16365 final Matrix b2 = new Matrix(3, 1);
16366 calibrator.getBiasAsMatrix(b2);
16367 assertEquals(b1, b2);
16368 final Matrix ma1 = calibrator.getInitialMa();
16369 assertEquals(ma1, new Matrix(3, 3));
16370 final Matrix ma2 = new Matrix(3, 3);
16371 calibrator.getInitialMa(ma2);
16372 assertEquals(ma1, ma2);
16373 assertSame(calibrator.getMeasurements(), measurements);
16374 assertFalse(calibrator.isCommonAxisUsed());
16375 assertSame(calibrator.getListener(), this);
16376 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16377 assertFalse(calibrator.isReady());
16378 assertFalse(calibrator.isRunning());
16379 assertNull(calibrator.getEstimatedMa());
16380 assertNull(calibrator.getEstimatedSx());
16381 assertNull(calibrator.getEstimatedSy());
16382 assertNull(calibrator.getEstimatedSz());
16383 assertNull(calibrator.getEstimatedMxy());
16384 assertNull(calibrator.getEstimatedMxz());
16385 assertNull(calibrator.getEstimatedMyx());
16386 assertNull(calibrator.getEstimatedMyz());
16387 assertNull(calibrator.getEstimatedMzx());
16388 assertNull(calibrator.getEstimatedMzy());
16389 assertNull(calibrator.getEstimatedCovariance());
16390 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16391 assertNotNull(calibrator.getGroundTruthGravityNorm());
16392 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16393 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16394 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16395 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16396 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16397 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16398 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16399
16400
16401 calibrator = null;
16402 try {
16403 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16404 -gravityNorm, measurements, ba, this);
16405 fail("IllegalArgumentException expected but not thrown");
16406 } catch (final IllegalArgumentException ignore) {
16407 }
16408 try {
16409 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16410 gravityNorm, measurements,
16411 new Matrix(1, 1), this);
16412 fail("IllegalArgumentException expected but not thrown");
16413 } catch (final IllegalArgumentException ignore) {
16414 }
16415 try {
16416 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16417 gravityNorm, measurements,
16418 new Matrix(1, 3), this);
16419 fail("IllegalArgumentException expected but not thrown");
16420 } catch (final IllegalArgumentException ignore) {
16421 }
16422 assertNull(calibrator);
16423 }
16424
16425 @Test
16426 public void testConstructor145() throws WrongSizeException {
16427 final Matrix ba = generateBa();
16428 final double[] bias = ba.getBuffer();
16429 final double biasX = ba.getElementAtIndex(0);
16430 final double biasY = ba.getElementAtIndex(1);
16431 final double biasZ = ba.getElementAtIndex(2);
16432
16433 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16434 final double latitude = Math.toRadians(
16435 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16436 final double longitude = Math.toRadians(
16437 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16438 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16439 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16440 final NEDVelocity nedVelocity = new NEDVelocity();
16441 final ECEFPosition ecefPosition = new ECEFPosition();
16442 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16443 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16444 ecefPosition, ecefVelocity);
16445 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16446 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16447 final double gravityNorm = gravity.getNorm();
16448
16449 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16450 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
16451 true, ba);
16452
16453
16454 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16455 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16456 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16457 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16458 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16459 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16460 final Acceleration bx2 = new Acceleration(0.0,
16461 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16462 calibrator.getBiasXAsAcceleration(bx2);
16463 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16464 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16465 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16466 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16467 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16468 final Acceleration by2 = new Acceleration(0.0,
16469 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16470 calibrator.getBiasYAsAcceleration(by2);
16471 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16472 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16473 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16474 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16475 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16476 final Acceleration bz2 = new Acceleration(0.0,
16477 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16478 calibrator.getBiasZAsAcceleration(bz2);
16479 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16480 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16481 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16482 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16483 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16484 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16485 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16486 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16487 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16488 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16489 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16490 final double[] bias1 = calibrator.getBias();
16491 assertArrayEquals(bias1, bias, 0.0);
16492 final double[] bias2 = new double[3];
16493 calibrator.getBias(bias2);
16494 assertArrayEquals(bias1, bias2, 0.0);
16495 final Matrix b1 = calibrator.getBiasAsMatrix();
16496 assertEquals(b1, ba);
16497 final Matrix b2 = new Matrix(3, 1);
16498 calibrator.getBiasAsMatrix(b2);
16499 assertEquals(b1, b2);
16500 final Matrix ma1 = calibrator.getInitialMa();
16501 assertEquals(ma1, new Matrix(3, 3));
16502 final Matrix ma2 = new Matrix(3, 3);
16503 calibrator.getInitialMa(ma2);
16504 assertEquals(ma1, ma2);
16505 assertNull(calibrator.getMeasurements());
16506 assertTrue(calibrator.isCommonAxisUsed());
16507 assertNull(calibrator.getListener());
16508 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16509 assertFalse(calibrator.isReady());
16510 assertFalse(calibrator.isRunning());
16511 assertNull(calibrator.getEstimatedMa());
16512 assertNull(calibrator.getEstimatedSx());
16513 assertNull(calibrator.getEstimatedSy());
16514 assertNull(calibrator.getEstimatedSz());
16515 assertNull(calibrator.getEstimatedMxy());
16516 assertNull(calibrator.getEstimatedMxz());
16517 assertNull(calibrator.getEstimatedMyx());
16518 assertNull(calibrator.getEstimatedMyz());
16519 assertNull(calibrator.getEstimatedMzx());
16520 assertNull(calibrator.getEstimatedMzy());
16521 assertNull(calibrator.getEstimatedCovariance());
16522 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16523 assertNotNull(calibrator.getGroundTruthGravityNorm());
16524 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16525 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16526 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16527 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16528 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16529 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16530 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16531
16532
16533 calibrator = null;
16534 try {
16535 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16536 -gravityNorm, true, ba);
16537 fail("IllegalArgumentException expected but not thrown");
16538 } catch (final IllegalArgumentException ignore) {
16539 }
16540 try {
16541 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16542 gravityNorm, true,
16543 new Matrix(1, 1));
16544 fail("IllegalArgumentException expected but not thrown");
16545 } catch (final IllegalArgumentException ignore) {
16546 }
16547 try {
16548 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16549 gravityNorm, true,
16550 new Matrix(1, 3));
16551 fail("IllegalArgumentException expected but not thrown");
16552 } catch (final IllegalArgumentException ignore) {
16553 }
16554 assertNull(calibrator);
16555 }
16556
16557 @Test
16558 public void testConstructor146() throws WrongSizeException {
16559 final Matrix ba = generateBa();
16560 final double[] bias = ba.getBuffer();
16561 final double biasX = ba.getElementAtIndex(0);
16562 final double biasY = ba.getElementAtIndex(1);
16563 final double biasZ = ba.getElementAtIndex(2);
16564
16565 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16566 final double latitude = Math.toRadians(
16567 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16568 final double longitude = Math.toRadians(
16569 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16570 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16571 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16572 final NEDVelocity nedVelocity = new NEDVelocity();
16573 final ECEFPosition ecefPosition = new ECEFPosition();
16574 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16575 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16576 ecefPosition, ecefVelocity);
16577 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16578 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16579 final double gravityNorm = gravity.getNorm();
16580
16581 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16582 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
16583 true, ba, this);
16584
16585
16586 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16587 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16588 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16589 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16590 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16591 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16592 final Acceleration bx2 = new Acceleration(0.0,
16593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16594 calibrator.getBiasXAsAcceleration(bx2);
16595 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16596 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16597 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16598 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16599 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16600 final Acceleration by2 = new Acceleration(0.0,
16601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16602 calibrator.getBiasYAsAcceleration(by2);
16603 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16604 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16605 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16606 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16607 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16608 final Acceleration bz2 = new Acceleration(0.0,
16609 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16610 calibrator.getBiasZAsAcceleration(bz2);
16611 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16612 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16613 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16614 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16615 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16616 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16617 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16618 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16619 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16620 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16621 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16622 final double[] bias1 = calibrator.getBias();
16623 assertArrayEquals(bias1, bias, 0.0);
16624 final double[] bias2 = new double[3];
16625 calibrator.getBias(bias2);
16626 assertArrayEquals(bias1, bias2, 0.0);
16627 final Matrix b1 = calibrator.getBiasAsMatrix();
16628 assertEquals(b1, ba);
16629 final Matrix b2 = new Matrix(3, 1);
16630 calibrator.getBiasAsMatrix(b2);
16631 assertEquals(b1, b2);
16632 final Matrix ma1 = calibrator.getInitialMa();
16633 assertEquals(ma1, new Matrix(3, 3));
16634 final Matrix ma2 = new Matrix(3, 3);
16635 calibrator.getInitialMa(ma2);
16636 assertEquals(ma1, ma2);
16637 assertNull(calibrator.getMeasurements());
16638 assertTrue(calibrator.isCommonAxisUsed());
16639 assertSame(calibrator.getListener(), this);
16640 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16641 assertFalse(calibrator.isReady());
16642 assertFalse(calibrator.isRunning());
16643 assertNull(calibrator.getEstimatedMa());
16644 assertNull(calibrator.getEstimatedSx());
16645 assertNull(calibrator.getEstimatedSy());
16646 assertNull(calibrator.getEstimatedSz());
16647 assertNull(calibrator.getEstimatedMxy());
16648 assertNull(calibrator.getEstimatedMxz());
16649 assertNull(calibrator.getEstimatedMyx());
16650 assertNull(calibrator.getEstimatedMyz());
16651 assertNull(calibrator.getEstimatedMzx());
16652 assertNull(calibrator.getEstimatedMzy());
16653 assertNull(calibrator.getEstimatedCovariance());
16654 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16655 assertNotNull(calibrator.getGroundTruthGravityNorm());
16656 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16657 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16658 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16659 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16660 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16661 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16662 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16663
16664
16665 calibrator = null;
16666 try {
16667 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16668 -gravityNorm, true, ba, this);
16669 fail("IllegalArgumentException expected but not thrown");
16670 } catch (final IllegalArgumentException ignore) {
16671 }
16672 try {
16673 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16674 gravityNorm, true, new Matrix(1, 1),
16675 this);
16676 fail("IllegalArgumentException expected but not thrown");
16677 } catch (final IllegalArgumentException ignore) {
16678 }
16679 try {
16680 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16681 gravityNorm, true, new Matrix(1, 3),
16682 this);
16683 fail("IllegalArgumentException expected but not thrown");
16684 } catch (final IllegalArgumentException ignore) {
16685 }
16686 assertNull(calibrator);
16687 }
16688
16689 @Test
16690 public void testConstructor147() throws WrongSizeException {
16691 final Collection<StandardDeviationBodyKinematics> measurements =
16692 Collections.emptyList();
16693
16694 final Matrix ba = generateBa();
16695 final double[] bias = ba.getBuffer();
16696 final double biasX = ba.getElementAtIndex(0);
16697 final double biasY = ba.getElementAtIndex(1);
16698 final double biasZ = ba.getElementAtIndex(2);
16699
16700 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16701 final double latitude = Math.toRadians(
16702 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16703 final double longitude = Math.toRadians(
16704 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16705 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16706 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16707 final NEDVelocity nedVelocity = new NEDVelocity();
16708 final ECEFPosition ecefPosition = new ECEFPosition();
16709 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16710 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16711 ecefPosition, ecefVelocity);
16712 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16713 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16714 final double gravityNorm = gravity.getNorm();
16715
16716 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16717 new KnownBiasAndGravityNormAccelerometerCalibrator(
16718 gravityNorm, measurements,
16719 true, ba);
16720
16721
16722 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16723 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16724 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16725 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16726 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16727 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16728 final Acceleration bx2 = new Acceleration(0.0,
16729 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16730 calibrator.getBiasXAsAcceleration(bx2);
16731 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16732 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16733 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16734 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16735 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16736 final Acceleration by2 = new Acceleration(0.0,
16737 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16738 calibrator.getBiasYAsAcceleration(by2);
16739 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16740 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16741 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16742 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16743 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16744 final Acceleration bz2 = new Acceleration(0.0,
16745 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16746 calibrator.getBiasZAsAcceleration(bz2);
16747 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16748 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16749 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16750 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16751 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16752 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16753 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16754 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16755 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16756 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16757 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16758 final double[] bias1 = calibrator.getBias();
16759 assertArrayEquals(bias1, bias, 0.0);
16760 final double[] bias2 = new double[3];
16761 calibrator.getBias(bias2);
16762 assertArrayEquals(bias1, bias2, 0.0);
16763 final Matrix b1 = calibrator.getBiasAsMatrix();
16764 assertEquals(b1, ba);
16765 final Matrix b2 = new Matrix(3, 1);
16766 calibrator.getBiasAsMatrix(b2);
16767 assertEquals(b1, b2);
16768 final Matrix ma1 = calibrator.getInitialMa();
16769 assertEquals(ma1, new Matrix(3, 3));
16770 final Matrix ma2 = new Matrix(3, 3);
16771 calibrator.getInitialMa(ma2);
16772 assertEquals(ma1, ma2);
16773 assertSame(calibrator.getMeasurements(), measurements);
16774 assertTrue(calibrator.isCommonAxisUsed());
16775 assertNull(calibrator.getListener());
16776 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16777 assertFalse(calibrator.isReady());
16778 assertFalse(calibrator.isRunning());
16779 assertNull(calibrator.getEstimatedMa());
16780 assertNull(calibrator.getEstimatedSx());
16781 assertNull(calibrator.getEstimatedSy());
16782 assertNull(calibrator.getEstimatedSz());
16783 assertNull(calibrator.getEstimatedMxy());
16784 assertNull(calibrator.getEstimatedMxz());
16785 assertNull(calibrator.getEstimatedMyx());
16786 assertNull(calibrator.getEstimatedMyz());
16787 assertNull(calibrator.getEstimatedMzx());
16788 assertNull(calibrator.getEstimatedMzy());
16789 assertNull(calibrator.getEstimatedCovariance());
16790 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16791 assertNotNull(calibrator.getGroundTruthGravityNorm());
16792 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16793 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16794 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16795 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16796 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16797 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16798 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16799
16800
16801 calibrator = null;
16802 try {
16803 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16804 -gravityNorm, measurements,
16805 true, ba);
16806 fail("IllegalArgumentException expected but not thrown");
16807 } catch (final IllegalArgumentException ignore) {
16808 }
16809 try {
16810 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16811 gravityNorm, measurements, true,
16812 new Matrix(1, 1));
16813 fail("IllegalArgumentException expected but not thrown");
16814 } catch (final IllegalArgumentException ignore) {
16815 }
16816 try {
16817 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16818 gravityNorm, measurements, true,
16819 new Matrix(1, 3));
16820 fail("IllegalArgumentException expected but not thrown");
16821 } catch (final IllegalArgumentException ignore) {
16822 }
16823 assertNull(calibrator);
16824 }
16825
16826 @Test
16827 public void testConstructor148() throws WrongSizeException {
16828 final Collection<StandardDeviationBodyKinematics> measurements =
16829 Collections.emptyList();
16830
16831 final Matrix ba = generateBa();
16832 final double[] bias = ba.getBuffer();
16833 final double biasX = ba.getElementAtIndex(0);
16834 final double biasY = ba.getElementAtIndex(1);
16835 final double biasZ = ba.getElementAtIndex(2);
16836
16837 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16838 final double latitude = Math.toRadians(
16839 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16840 final double longitude = Math.toRadians(
16841 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16842 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16843 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16844 final NEDVelocity nedVelocity = new NEDVelocity();
16845 final ECEFPosition ecefPosition = new ECEFPosition();
16846 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16847 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16848 ecefPosition, ecefVelocity);
16849 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16850 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16851 final double gravityNorm = gravity.getNorm();
16852
16853 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16854 new KnownBiasAndGravityNormAccelerometerCalibrator(
16855 gravityNorm, measurements,
16856 true, ba, this);
16857
16858
16859 assertEquals(calibrator.getBiasX(), biasX, 0.0);
16860 assertEquals(calibrator.getBiasY(), biasY, 0.0);
16861 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16862 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16863 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16864 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16865 final Acceleration bx2 = new Acceleration(0.0,
16866 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16867 calibrator.getBiasXAsAcceleration(bx2);
16868 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16869 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16870 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16871 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16872 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16873 final Acceleration by2 = new Acceleration(0.0,
16874 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16875 calibrator.getBiasYAsAcceleration(by2);
16876 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16877 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16878 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16879 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16880 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16881 final Acceleration bz2 = new Acceleration(0.0,
16882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16883 calibrator.getBiasZAsAcceleration(bz2);
16884 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16885 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16886 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16887 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16888 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16889 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16890 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16891 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16892 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16893 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16894 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16895 final double[] bias1 = calibrator.getBias();
16896 assertArrayEquals(bias1, bias, 0.0);
16897 final double[] bias2 = new double[3];
16898 calibrator.getBias(bias2);
16899 assertArrayEquals(bias1, bias2, 0.0);
16900 final Matrix b1 = calibrator.getBiasAsMatrix();
16901 assertEquals(b1, ba);
16902 final Matrix b2 = new Matrix(3, 1);
16903 calibrator.getBiasAsMatrix(b2);
16904 assertEquals(b1, b2);
16905 final Matrix ma1 = calibrator.getInitialMa();
16906 assertEquals(ma1, new Matrix(3, 3));
16907 final Matrix ma2 = new Matrix(3, 3);
16908 calibrator.getInitialMa(ma2);
16909 assertEquals(ma1, ma2);
16910 assertSame(calibrator.getMeasurements(), measurements);
16911 assertTrue(calibrator.isCommonAxisUsed());
16912 assertSame(calibrator.getListener(), this);
16913 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16914 assertFalse(calibrator.isReady());
16915 assertFalse(calibrator.isRunning());
16916 assertNull(calibrator.getEstimatedMa());
16917 assertNull(calibrator.getEstimatedSx());
16918 assertNull(calibrator.getEstimatedSy());
16919 assertNull(calibrator.getEstimatedSz());
16920 assertNull(calibrator.getEstimatedMxy());
16921 assertNull(calibrator.getEstimatedMxz());
16922 assertNull(calibrator.getEstimatedMyx());
16923 assertNull(calibrator.getEstimatedMyz());
16924 assertNull(calibrator.getEstimatedMzx());
16925 assertNull(calibrator.getEstimatedMzy());
16926 assertNull(calibrator.getEstimatedCovariance());
16927 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16928 assertNotNull(calibrator.getGroundTruthGravityNorm());
16929 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16930 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16931 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16932 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16933 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16934 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16935 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16936
16937
16938 calibrator = null;
16939 try {
16940 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16941 -gravityNorm, measurements,
16942 true, ba, this);
16943 fail("IllegalArgumentException expected but not thrown");
16944 } catch (final IllegalArgumentException ignore) {
16945 }
16946 try {
16947 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16948 gravityNorm, measurements, true,
16949 new Matrix(1, 1), this);
16950 fail("IllegalArgumentException expected but not thrown");
16951 } catch (final IllegalArgumentException ignore) {
16952 }
16953 try {
16954 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16955 gravityNorm, measurements, true,
16956 new Matrix(1, 3), this);
16957 fail("IllegalArgumentException expected but not thrown");
16958 } catch (final IllegalArgumentException ignore) {
16959 }
16960 assertNull(calibrator);
16961 }
16962
16963 @Test
16964 public void testConstructor149() throws WrongSizeException {
16965 final Matrix ba = generateBa();
16966 final double[] bias = ba.getBuffer();
16967 final double biasX = ba.getElementAtIndex(0);
16968 final double biasY = ba.getElementAtIndex(1);
16969 final double biasZ = ba.getElementAtIndex(2);
16970
16971 final Matrix ma = generateMaCommonAxis();
16972 final double sx = ma.getElementAt(0, 0);
16973 final double sy = ma.getElementAt(1, 1);
16974 final double sz = ma.getElementAt(2, 2);
16975 final double mxy = ma.getElementAt(0, 1);
16976 final double mxz = ma.getElementAt(0, 2);
16977 final double myx = ma.getElementAt(1, 0);
16978 final double myz = ma.getElementAt(1, 2);
16979 final double mzx = ma.getElementAt(2, 0);
16980 final double mzy = ma.getElementAt(2, 1);
16981
16982 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16983 final double latitude = Math.toRadians(
16984 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16985 final double longitude = Math.toRadians(
16986 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16987 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16988 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16989 final NEDVelocity nedVelocity = new NEDVelocity();
16990 final ECEFPosition ecefPosition = new ECEFPosition();
16991 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16992 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16993 ecefPosition, ecefVelocity);
16994 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16995 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16996 final double gravityNorm = gravity.getNorm();
16997
16998 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16999 new KnownBiasAndGravityNormAccelerometerCalibrator(
17000 gravityNorm, ba, ma);
17001
17002
17003 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17004 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17005 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17006 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17007 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17008 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17009 final Acceleration bx2 = new Acceleration(0.0,
17010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17011 calibrator.getBiasXAsAcceleration(bx2);
17012 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17013 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17014 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17015 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17016 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17017 final Acceleration by2 = new Acceleration(0.0,
17018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17019 calibrator.getBiasYAsAcceleration(by2);
17020 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17021 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17022 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17023 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17024 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17025 final Acceleration bz2 = new Acceleration(0.0,
17026 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17027 calibrator.getBiasZAsAcceleration(bz2);
17028 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17029 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17030 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17031 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17032 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17033 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17034 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17035 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17036 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17037 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17038 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17039 final double[] bias1 = calibrator.getBias();
17040 assertArrayEquals(bias1, bias, 0.0);
17041 final double[] bias2 = new double[3];
17042 calibrator.getBias(bias2);
17043 assertArrayEquals(bias1, bias2, 0.0);
17044 final Matrix b1 = calibrator.getBiasAsMatrix();
17045 assertEquals(b1, ba);
17046 final Matrix b2 = new Matrix(3, 1);
17047 calibrator.getBiasAsMatrix(b2);
17048 assertEquals(b1, b2);
17049 final Matrix ma1 = new Matrix(3, 3);
17050 ma1.setSubmatrix(0, 0,
17051 2, 2,
17052 new double[]{sx, myx, mzx,
17053 mxy, sy, mzy,
17054 mxz, myz, sz});
17055 assertEquals(calibrator.getInitialMa(), ma1);
17056 final Matrix ma2 = new Matrix(3, 3);
17057 calibrator.getInitialMa(ma2);
17058 assertEquals(ma1, ma2);
17059 assertNull(calibrator.getMeasurements());
17060 assertFalse(calibrator.isCommonAxisUsed());
17061 assertNull(calibrator.getListener());
17062 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17063 assertFalse(calibrator.isReady());
17064 assertFalse(calibrator.isRunning());
17065 assertNull(calibrator.getEstimatedMa());
17066 assertNull(calibrator.getEstimatedSx());
17067 assertNull(calibrator.getEstimatedSy());
17068 assertNull(calibrator.getEstimatedSz());
17069 assertNull(calibrator.getEstimatedMxy());
17070 assertNull(calibrator.getEstimatedMxz());
17071 assertNull(calibrator.getEstimatedMyx());
17072 assertNull(calibrator.getEstimatedMyz());
17073 assertNull(calibrator.getEstimatedMzx());
17074 assertNull(calibrator.getEstimatedMzy());
17075 assertNull(calibrator.getEstimatedCovariance());
17076 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17077 assertNotNull(calibrator.getGroundTruthGravityNorm());
17078 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17079 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17080 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17081 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17082 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17083 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17084 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17085
17086
17087 calibrator = null;
17088 try {
17089 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17090 -gravityNorm, ba, ma);
17091 fail("IllegalArgumentException expected but not thrown");
17092 } catch (final IllegalArgumentException ignore) {
17093 }
17094 try {
17095 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17096 gravityNorm, new Matrix(1, 1), ma);
17097 fail("IllegalArgumentException expected but not thrown");
17098 } catch (final IllegalArgumentException ignore) {
17099 }
17100 try {
17101 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17102 gravityNorm, new Matrix(1, 3), ma);
17103 fail("IllegalArgumentException expected but not thrown");
17104 } catch (final IllegalArgumentException ignore) {
17105 }
17106 try {
17107 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17108 gravityNorm, ba, new Matrix(1, 3));
17109 fail("IllegalArgumentException expected but not thrown");
17110 } catch (final IllegalArgumentException ignore) {
17111 }
17112 try {
17113 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17114 gravityNorm, ba, new Matrix(3, 1));
17115 fail("IllegalArgumentException expected but not thrown");
17116 } catch (final IllegalArgumentException ignore) {
17117 }
17118 assertNull(calibrator);
17119 }
17120
17121 @Test
17122 public void testConstructor150() throws WrongSizeException {
17123 final Matrix ba = generateBa();
17124 final double[] bias = ba.getBuffer();
17125 final double biasX = ba.getElementAtIndex(0);
17126 final double biasY = ba.getElementAtIndex(1);
17127 final double biasZ = ba.getElementAtIndex(2);
17128
17129 final Matrix ma = generateMaCommonAxis();
17130 final double sx = ma.getElementAt(0, 0);
17131 final double sy = ma.getElementAt(1, 1);
17132 final double sz = ma.getElementAt(2, 2);
17133 final double mxy = ma.getElementAt(0, 1);
17134 final double mxz = ma.getElementAt(0, 2);
17135 final double myx = ma.getElementAt(1, 0);
17136 final double myz = ma.getElementAt(1, 2);
17137 final double mzx = ma.getElementAt(2, 0);
17138 final double mzy = ma.getElementAt(2, 1);
17139
17140 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17141 final double latitude = Math.toRadians(
17142 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17143 final double longitude = Math.toRadians(
17144 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17145 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17146 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17147 final NEDVelocity nedVelocity = new NEDVelocity();
17148 final ECEFPosition ecefPosition = new ECEFPosition();
17149 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17150 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17151 ecefPosition, ecefVelocity);
17152 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17153 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17154 final double gravityNorm = gravity.getNorm();
17155
17156 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17157 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17158 ba, ma, this);
17159
17160
17161 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17162 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17163 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17164 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17165 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17166 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17167 final Acceleration bx2 = new Acceleration(0.0,
17168 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17169 calibrator.getBiasXAsAcceleration(bx2);
17170 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17171 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17172 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17173 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17174 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17175 final Acceleration by2 = new Acceleration(0.0,
17176 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17177 calibrator.getBiasYAsAcceleration(by2);
17178 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17179 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17180 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17181 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17182 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17183 final Acceleration bz2 = new Acceleration(0.0,
17184 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17185 calibrator.getBiasZAsAcceleration(bz2);
17186 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17187 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17188 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17189 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17190 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17191 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17192 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17193 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17194 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17195 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17196 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17197 final double[] bias1 = calibrator.getBias();
17198 assertArrayEquals(bias1, bias, 0.0);
17199 final double[] bias2 = new double[3];
17200 calibrator.getBias(bias2);
17201 assertArrayEquals(bias1, bias2, 0.0);
17202 final Matrix b1 = calibrator.getBiasAsMatrix();
17203 assertEquals(b1, ba);
17204 final Matrix b2 = new Matrix(3, 1);
17205 calibrator.getBiasAsMatrix(b2);
17206 assertEquals(b1, b2);
17207 final Matrix ma1 = new Matrix(3, 3);
17208 ma1.setSubmatrix(0, 0,
17209 2, 2,
17210 new double[]{sx, myx, mzx,
17211 mxy, sy, mzy,
17212 mxz, myz, sz});
17213 assertEquals(calibrator.getInitialMa(), ma1);
17214 final Matrix ma2 = new Matrix(3, 3);
17215 calibrator.getInitialMa(ma2);
17216 assertEquals(ma1, ma2);
17217 assertNull(calibrator.getMeasurements());
17218 assertFalse(calibrator.isCommonAxisUsed());
17219 assertSame(calibrator.getListener(), this);
17220 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17221 assertFalse(calibrator.isReady());
17222 assertFalse(calibrator.isRunning());
17223 assertNull(calibrator.getEstimatedMa());
17224 assertNull(calibrator.getEstimatedSx());
17225 assertNull(calibrator.getEstimatedSy());
17226 assertNull(calibrator.getEstimatedSz());
17227 assertNull(calibrator.getEstimatedMxy());
17228 assertNull(calibrator.getEstimatedMxz());
17229 assertNull(calibrator.getEstimatedMyx());
17230 assertNull(calibrator.getEstimatedMyz());
17231 assertNull(calibrator.getEstimatedMzx());
17232 assertNull(calibrator.getEstimatedMzy());
17233 assertNull(calibrator.getEstimatedCovariance());
17234 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17235 assertNotNull(calibrator.getGroundTruthGravityNorm());
17236 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17237 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17238 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17239 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17240 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17241 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17242 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17243
17244
17245 calibrator = null;
17246 try {
17247 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17248 -gravityNorm, ba, ma, this);
17249 fail("IllegalArgumentException expected but not thrown");
17250 } catch (final IllegalArgumentException ignore) {
17251 }
17252 try {
17253 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17254 gravityNorm, new Matrix(1, 1), ma,
17255 this);
17256 fail("IllegalArgumentException expected but not thrown");
17257 } catch (final IllegalArgumentException ignore) {
17258 }
17259 try {
17260 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17261 gravityNorm, new Matrix(1, 3), ma,
17262 this);
17263 fail("IllegalArgumentException expected but not thrown");
17264 } catch (final IllegalArgumentException ignore) {
17265 }
17266 try {
17267 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17268 gravityNorm, ba, new Matrix(1, 3), this);
17269 fail("IllegalArgumentException expected but not thrown");
17270 } catch (final IllegalArgumentException ignore) {
17271 }
17272 try {
17273 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17274 gravityNorm, ba, new Matrix(3, 1), this);
17275 fail("IllegalArgumentException expected but not thrown");
17276 } catch (final IllegalArgumentException ignore) {
17277 }
17278 assertNull(calibrator);
17279 }
17280
17281 @Test
17282 public void testConstructor151() throws WrongSizeException {
17283 final Collection<StandardDeviationBodyKinematics> measurements =
17284 Collections.emptyList();
17285
17286 final Matrix ba = generateBa();
17287 final double[] bias = ba.getBuffer();
17288 final double biasX = ba.getElementAtIndex(0);
17289 final double biasY = ba.getElementAtIndex(1);
17290 final double biasZ = ba.getElementAtIndex(2);
17291
17292 final Matrix ma = generateMaCommonAxis();
17293 final double sx = ma.getElementAt(0, 0);
17294 final double sy = ma.getElementAt(1, 1);
17295 final double sz = ma.getElementAt(2, 2);
17296 final double mxy = ma.getElementAt(0, 1);
17297 final double mxz = ma.getElementAt(0, 2);
17298 final double myx = ma.getElementAt(1, 0);
17299 final double myz = ma.getElementAt(1, 2);
17300 final double mzx = ma.getElementAt(2, 0);
17301 final double mzy = ma.getElementAt(2, 1);
17302
17303 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17304 final double latitude = Math.toRadians(
17305 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17306 final double longitude = Math.toRadians(
17307 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17308 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17309 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17310 final NEDVelocity nedVelocity = new NEDVelocity();
17311 final ECEFPosition ecefPosition = new ECEFPosition();
17312 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17313 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17314 ecefPosition, ecefVelocity);
17315 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17316 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17317 final double gravityNorm = gravity.getNorm();
17318
17319 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17320 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17321 measurements, ba, ma);
17322
17323
17324 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17325 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17326 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17327 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17328 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17329 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17330 final Acceleration bx2 = new Acceleration(0.0,
17331 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17332 calibrator.getBiasXAsAcceleration(bx2);
17333 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17334 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17335 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17336 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17337 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17338 final Acceleration by2 = new Acceleration(0.0,
17339 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17340 calibrator.getBiasYAsAcceleration(by2);
17341 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17342 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17343 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17344 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17345 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17346 final Acceleration bz2 = new Acceleration(0.0,
17347 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17348 calibrator.getBiasZAsAcceleration(bz2);
17349 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17350 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17351 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17352 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17353 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17354 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17355 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17356 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17357 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17358 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17359 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17360 final double[] bias1 = calibrator.getBias();
17361 assertArrayEquals(bias1, bias, 0.0);
17362 final double[] bias2 = new double[3];
17363 calibrator.getBias(bias2);
17364 assertArrayEquals(bias1, bias2, 0.0);
17365 final Matrix b1 = calibrator.getBiasAsMatrix();
17366 assertEquals(b1, ba);
17367 final Matrix b2 = new Matrix(3, 1);
17368 calibrator.getBiasAsMatrix(b2);
17369 assertEquals(b1, b2);
17370 final Matrix ma1 = new Matrix(3, 3);
17371 ma1.setSubmatrix(0, 0,
17372 2, 2,
17373 new double[]{sx, myx, mzx,
17374 mxy, sy, mzy,
17375 mxz, myz, sz});
17376 assertEquals(calibrator.getInitialMa(), ma1);
17377 final Matrix ma2 = new Matrix(3, 3);
17378 calibrator.getInitialMa(ma2);
17379 assertEquals(ma1, ma2);
17380 assertSame(calibrator.getMeasurements(), measurements);
17381 assertFalse(calibrator.isCommonAxisUsed());
17382 assertNull(calibrator.getListener());
17383 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17384 assertFalse(calibrator.isReady());
17385 assertFalse(calibrator.isRunning());
17386 assertNull(calibrator.getEstimatedMa());
17387 assertNull(calibrator.getEstimatedSx());
17388 assertNull(calibrator.getEstimatedSy());
17389 assertNull(calibrator.getEstimatedSz());
17390 assertNull(calibrator.getEstimatedMxy());
17391 assertNull(calibrator.getEstimatedMxz());
17392 assertNull(calibrator.getEstimatedMyx());
17393 assertNull(calibrator.getEstimatedMyz());
17394 assertNull(calibrator.getEstimatedMzx());
17395 assertNull(calibrator.getEstimatedMzy());
17396 assertNull(calibrator.getEstimatedCovariance());
17397 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17398 assertNotNull(calibrator.getGroundTruthGravityNorm());
17399 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17400 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17401 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17402 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17403 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17404 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17405 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17406
17407
17408 calibrator = null;
17409 try {
17410 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17411 -gravityNorm, measurements, ba, ma);
17412 fail("IllegalArgumentException expected but not thrown");
17413 } catch (final IllegalArgumentException ignore) {
17414 }
17415 try {
17416 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17417 gravityNorm, measurements,
17418 new Matrix(1, 1), ma);
17419 fail("IllegalArgumentException expected but not thrown");
17420 } catch (final IllegalArgumentException ignore) {
17421 }
17422 try {
17423 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17424 gravityNorm, measurements,
17425 new Matrix(1, 3), ma);
17426 fail("IllegalArgumentException expected but not thrown");
17427 } catch (final IllegalArgumentException ignore) {
17428 }
17429 try {
17430 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17431 gravityNorm, measurements, ba,
17432 new Matrix(1, 3));
17433 fail("IllegalArgumentException expected but not thrown");
17434 } catch (final IllegalArgumentException ignore) {
17435 }
17436 try {
17437 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17438 gravityNorm, measurements, ba,
17439 new Matrix(3, 1));
17440 fail("IllegalArgumentException expected but not thrown");
17441 } catch (final IllegalArgumentException ignore) {
17442 }
17443 assertNull(calibrator);
17444 }
17445
17446 @Test
17447 public void testConstructor152() throws WrongSizeException {
17448 final Collection<StandardDeviationBodyKinematics> measurements =
17449 Collections.emptyList();
17450
17451 final Matrix ba = generateBa();
17452 final double[] bias = ba.getBuffer();
17453 final double biasX = ba.getElementAtIndex(0);
17454 final double biasY = ba.getElementAtIndex(1);
17455 final double biasZ = ba.getElementAtIndex(2);
17456
17457 final Matrix ma = generateMaCommonAxis();
17458 final double sx = ma.getElementAt(0, 0);
17459 final double sy = ma.getElementAt(1, 1);
17460 final double sz = ma.getElementAt(2, 2);
17461 final double mxy = ma.getElementAt(0, 1);
17462 final double mxz = ma.getElementAt(0, 2);
17463 final double myx = ma.getElementAt(1, 0);
17464 final double myz = ma.getElementAt(1, 2);
17465 final double mzx = ma.getElementAt(2, 0);
17466 final double mzy = ma.getElementAt(2, 1);
17467
17468 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17469 final double latitude = Math.toRadians(
17470 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17471 final double longitude = Math.toRadians(
17472 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17473 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17474 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17475 final NEDVelocity nedVelocity = new NEDVelocity();
17476 final ECEFPosition ecefPosition = new ECEFPosition();
17477 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17478 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17479 ecefPosition, ecefVelocity);
17480 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17481 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17482 final double gravityNorm = gravity.getNorm();
17483
17484 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17485 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17486 measurements, ba, ma, this);
17487
17488
17489 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17490 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17491 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17492 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17493 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17494 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17495 final Acceleration bx2 = new Acceleration(0.0,
17496 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17497 calibrator.getBiasXAsAcceleration(bx2);
17498 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17499 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17500 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17501 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17502 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17503 final Acceleration by2 = new Acceleration(0.0,
17504 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17505 calibrator.getBiasYAsAcceleration(by2);
17506 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17507 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17508 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17509 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17510 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17511 final Acceleration bz2 = new Acceleration(0.0,
17512 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17513 calibrator.getBiasZAsAcceleration(bz2);
17514 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17515 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17516 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17517 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17518 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17519 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17520 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17521 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17522 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17523 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17524 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17525 final double[] bias1 = calibrator.getBias();
17526 assertArrayEquals(bias1, bias, 0.0);
17527 final double[] bias2 = new double[3];
17528 calibrator.getBias(bias2);
17529 assertArrayEquals(bias1, bias2, 0.0);
17530 final Matrix b1 = calibrator.getBiasAsMatrix();
17531 assertEquals(b1, ba);
17532 final Matrix b2 = new Matrix(3, 1);
17533 calibrator.getBiasAsMatrix(b2);
17534 assertEquals(b1, b2);
17535 final Matrix ma1 = new Matrix(3, 3);
17536 ma1.setSubmatrix(0, 0,
17537 2, 2,
17538 new double[]{sx, myx, mzx,
17539 mxy, sy, mzy,
17540 mxz, myz, sz});
17541 assertEquals(calibrator.getInitialMa(), ma1);
17542 final Matrix ma2 = new Matrix(3, 3);
17543 calibrator.getInitialMa(ma2);
17544 assertEquals(ma1, ma2);
17545 assertSame(calibrator.getMeasurements(), measurements);
17546 assertFalse(calibrator.isCommonAxisUsed());
17547 assertSame(calibrator.getListener(), this);
17548 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17549 assertFalse(calibrator.isReady());
17550 assertFalse(calibrator.isRunning());
17551 assertNull(calibrator.getEstimatedMa());
17552 assertNull(calibrator.getEstimatedSx());
17553 assertNull(calibrator.getEstimatedSy());
17554 assertNull(calibrator.getEstimatedSz());
17555 assertNull(calibrator.getEstimatedMxy());
17556 assertNull(calibrator.getEstimatedMxz());
17557 assertNull(calibrator.getEstimatedMyx());
17558 assertNull(calibrator.getEstimatedMyz());
17559 assertNull(calibrator.getEstimatedMzx());
17560 assertNull(calibrator.getEstimatedMzy());
17561 assertNull(calibrator.getEstimatedCovariance());
17562 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17563 assertNotNull(calibrator.getGroundTruthGravityNorm());
17564 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17565 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17566 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17567 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17568 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17569 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17570 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17571
17572
17573 calibrator = null;
17574 try {
17575 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17576 -gravityNorm, measurements, ba, ma, this);
17577 fail("IllegalArgumentException expected but not thrown");
17578 } catch (final IllegalArgumentException ignore) {
17579 }
17580 try {
17581 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17582 gravityNorm, measurements,
17583 new Matrix(1, 1), ma, this);
17584 fail("IllegalArgumentException expected but not thrown");
17585 } catch (final IllegalArgumentException ignore) {
17586 }
17587 try {
17588 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17589 gravityNorm, measurements,
17590 new Matrix(1, 3), ma, this);
17591 fail("IllegalArgumentException expected but not thrown");
17592 } catch (final IllegalArgumentException ignore) {
17593 }
17594 try {
17595 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17596 gravityNorm, measurements, ba,
17597 new Matrix(1, 3), this);
17598 fail("IllegalArgumentException expected but not thrown");
17599 } catch (final IllegalArgumentException ignore) {
17600 }
17601 try {
17602 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17603 gravityNorm, measurements, ba,
17604 new Matrix(3, 1), this);
17605 fail("IllegalArgumentException expected but not thrown");
17606 } catch (final IllegalArgumentException ignore) {
17607 }
17608 assertNull(calibrator);
17609 }
17610
17611 @Test
17612 public void testConstructor153() throws WrongSizeException {
17613 final Matrix ba = generateBa();
17614 final double[] bias = ba.getBuffer();
17615 final double biasX = ba.getElementAtIndex(0);
17616 final double biasY = ba.getElementAtIndex(1);
17617 final double biasZ = ba.getElementAtIndex(2);
17618
17619 final Matrix ma = generateMaCommonAxis();
17620 final double sx = ma.getElementAt(0, 0);
17621 final double sy = ma.getElementAt(1, 1);
17622 final double sz = ma.getElementAt(2, 2);
17623 final double mxy = ma.getElementAt(0, 1);
17624 final double mxz = ma.getElementAt(0, 2);
17625 final double myx = ma.getElementAt(1, 0);
17626 final double myz = ma.getElementAt(1, 2);
17627 final double mzx = ma.getElementAt(2, 0);
17628 final double mzy = ma.getElementAt(2, 1);
17629
17630 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17631 final double latitude = Math.toRadians(
17632 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17633 final double longitude = Math.toRadians(
17634 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17635 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17636 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17637 final NEDVelocity nedVelocity = new NEDVelocity();
17638 final ECEFPosition ecefPosition = new ECEFPosition();
17639 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17640 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17641 ecefPosition, ecefVelocity);
17642 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17643 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17644 final double gravityNorm = gravity.getNorm();
17645
17646 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17647 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17648 true, ba, ma);
17649
17650
17651 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17652 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17653 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17654 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17655 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17656 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17657 final Acceleration bx2 = new Acceleration(0.0,
17658 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17659 calibrator.getBiasXAsAcceleration(bx2);
17660 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17661 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17662 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17663 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17664 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17665 final Acceleration by2 = new Acceleration(0.0,
17666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17667 calibrator.getBiasYAsAcceleration(by2);
17668 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17669 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17670 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17671 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17672 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17673 final Acceleration bz2 = new Acceleration(0.0,
17674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17675 calibrator.getBiasZAsAcceleration(bz2);
17676 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17677 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17678 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17679 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17680 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17681 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17682 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17683 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17684 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17685 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17686 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17687 final double[] bias1 = calibrator.getBias();
17688 assertArrayEquals(bias1, bias, 0.0);
17689 final double[] bias2 = new double[3];
17690 calibrator.getBias(bias2);
17691 assertArrayEquals(bias1, bias2, 0.0);
17692 final Matrix b1 = calibrator.getBiasAsMatrix();
17693 assertEquals(b1, ba);
17694 final Matrix b2 = new Matrix(3, 1);
17695 calibrator.getBiasAsMatrix(b2);
17696 assertEquals(b1, b2);
17697 final Matrix ma1 = new Matrix(3, 3);
17698 ma1.setSubmatrix(0, 0,
17699 2, 2,
17700 new double[]{sx, myx, mzx,
17701 mxy, sy, mzy,
17702 mxz, myz, sz});
17703 assertEquals(calibrator.getInitialMa(), ma1);
17704 final Matrix ma2 = new Matrix(3, 3);
17705 calibrator.getInitialMa(ma2);
17706 assertEquals(ma1, ma2);
17707 assertNull(calibrator.getMeasurements());
17708 assertTrue(calibrator.isCommonAxisUsed());
17709 assertNull(calibrator.getListener());
17710 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
17711 assertFalse(calibrator.isReady());
17712 assertFalse(calibrator.isRunning());
17713 assertNull(calibrator.getEstimatedMa());
17714 assertNull(calibrator.getEstimatedSx());
17715 assertNull(calibrator.getEstimatedSy());
17716 assertNull(calibrator.getEstimatedSz());
17717 assertNull(calibrator.getEstimatedMxy());
17718 assertNull(calibrator.getEstimatedMxz());
17719 assertNull(calibrator.getEstimatedMyx());
17720 assertNull(calibrator.getEstimatedMyz());
17721 assertNull(calibrator.getEstimatedMzx());
17722 assertNull(calibrator.getEstimatedMzy());
17723 assertNull(calibrator.getEstimatedCovariance());
17724 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17725 assertNotNull(calibrator.getGroundTruthGravityNorm());
17726 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17727 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17728 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17729 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17730 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17731 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17732 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17733
17734
17735 calibrator = null;
17736 try {
17737 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17738 -gravityNorm, true, ba, ma);
17739 fail("IllegalArgumentException expected but not thrown");
17740 } catch (final IllegalArgumentException ignore) {
17741 }
17742 try {
17743 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17744 gravityNorm, true,
17745 new Matrix(1, 1), ma);
17746 fail("IllegalArgumentException expected but not thrown");
17747 } catch (final IllegalArgumentException ignore) {
17748 }
17749 try {
17750 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17751 gravityNorm, true,
17752 new Matrix(1, 3), ma);
17753 fail("IllegalArgumentException expected but not thrown");
17754 } catch (final IllegalArgumentException ignore) {
17755 }
17756 try {
17757 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17758 gravityNorm, true, ba,
17759 new Matrix(1, 3));
17760 fail("IllegalArgumentException expected but not thrown");
17761 } catch (final IllegalArgumentException ignore) {
17762 }
17763 try {
17764 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17765 gravityNorm, true, ba,
17766 new Matrix(3, 1));
17767 fail("IllegalArgumentException expected but not thrown");
17768 } catch (final IllegalArgumentException ignore) {
17769 }
17770 assertNull(calibrator);
17771 }
17772
17773 @Test
17774 public void testConstructor154() throws WrongSizeException {
17775 final Matrix ba = generateBa();
17776 final double[] bias = ba.getBuffer();
17777 final double biasX = ba.getElementAtIndex(0);
17778 final double biasY = ba.getElementAtIndex(1);
17779 final double biasZ = ba.getElementAtIndex(2);
17780
17781 final Matrix ma = generateMaCommonAxis();
17782 final double sx = ma.getElementAt(0, 0);
17783 final double sy = ma.getElementAt(1, 1);
17784 final double sz = ma.getElementAt(2, 2);
17785 final double mxy = ma.getElementAt(0, 1);
17786 final double mxz = ma.getElementAt(0, 2);
17787 final double myx = ma.getElementAt(1, 0);
17788 final double myz = ma.getElementAt(1, 2);
17789 final double mzx = ma.getElementAt(2, 0);
17790 final double mzy = ma.getElementAt(2, 1);
17791
17792 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17793 final double latitude = Math.toRadians(
17794 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17795 final double longitude = Math.toRadians(
17796 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17797 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17798 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17799 final NEDVelocity nedVelocity = new NEDVelocity();
17800 final ECEFPosition ecefPosition = new ECEFPosition();
17801 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17802 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17803 ecefPosition, ecefVelocity);
17804 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17805 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17806 final double gravityNorm = gravity.getNorm();
17807
17808 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17809 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17810 true, ba, ma, this);
17811
17812
17813 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17814 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17815 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17816 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17817 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17818 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17819 final Acceleration bx2 = new Acceleration(0.0,
17820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17821 calibrator.getBiasXAsAcceleration(bx2);
17822 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17823 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17824 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17825 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17826 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17827 final Acceleration by2 = new Acceleration(0.0,
17828 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17829 calibrator.getBiasYAsAcceleration(by2);
17830 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17831 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17832 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17833 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17834 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17835 final Acceleration bz2 = new Acceleration(0.0,
17836 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17837 calibrator.getBiasZAsAcceleration(bz2);
17838 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17839 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17840 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17841 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17842 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17843 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17844 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17845 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17846 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17847 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17848 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17849 final double[] bias1 = calibrator.getBias();
17850 assertArrayEquals(bias1, bias, 0.0);
17851 final double[] bias2 = new double[3];
17852 calibrator.getBias(bias2);
17853 assertArrayEquals(bias1, bias2, 0.0);
17854 final Matrix b1 = calibrator.getBiasAsMatrix();
17855 assertEquals(b1, ba);
17856 final Matrix b2 = new Matrix(3, 1);
17857 calibrator.getBiasAsMatrix(b2);
17858 assertEquals(b1, b2);
17859 final Matrix ma1 = new Matrix(3, 3);
17860 ma1.setSubmatrix(0, 0,
17861 2, 2,
17862 new double[]{sx, myx, mzx,
17863 mxy, sy, mzy,
17864 mxz, myz, sz});
17865 assertEquals(calibrator.getInitialMa(), ma1);
17866 final Matrix ma2 = new Matrix(3, 3);
17867 calibrator.getInitialMa(ma2);
17868 assertEquals(ma1, ma2);
17869 assertNull(calibrator.getMeasurements());
17870 assertTrue(calibrator.isCommonAxisUsed());
17871 assertSame(calibrator.getListener(), this);
17872 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
17873 assertFalse(calibrator.isReady());
17874 assertFalse(calibrator.isRunning());
17875 assertNull(calibrator.getEstimatedMa());
17876 assertNull(calibrator.getEstimatedSx());
17877 assertNull(calibrator.getEstimatedSy());
17878 assertNull(calibrator.getEstimatedSz());
17879 assertNull(calibrator.getEstimatedMxy());
17880 assertNull(calibrator.getEstimatedMxz());
17881 assertNull(calibrator.getEstimatedMyx());
17882 assertNull(calibrator.getEstimatedMyz());
17883 assertNull(calibrator.getEstimatedMzx());
17884 assertNull(calibrator.getEstimatedMzy());
17885 assertNull(calibrator.getEstimatedCovariance());
17886 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17887 assertNotNull(calibrator.getGroundTruthGravityNorm());
17888 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17889 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17890 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17891 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17892 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17893 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17894 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17895
17896
17897 calibrator = null;
17898 try {
17899 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17900 -gravityNorm, true, ba, ma, this);
17901 fail("IllegalArgumentException expected but not thrown");
17902 } catch (final IllegalArgumentException ignore) {
17903 }
17904 try {
17905 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17906 gravityNorm, true, new Matrix(1, 1),
17907 ma, this);
17908 fail("IllegalArgumentException expected but not thrown");
17909 } catch (final IllegalArgumentException ignore) {
17910 }
17911 try {
17912 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17913 gravityNorm, true, new Matrix(1, 3),
17914 ma, this);
17915 fail("IllegalArgumentException expected but not thrown");
17916 } catch (final IllegalArgumentException ignore) {
17917 }
17918 try {
17919 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17920 gravityNorm, true, ba, new Matrix(1, 3),
17921 this);
17922 fail("IllegalArgumentException expected but not thrown");
17923 } catch (final IllegalArgumentException ignore) {
17924 }
17925 try {
17926 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17927 gravityNorm, true, ba, new Matrix(3, 1),
17928 this);
17929 fail("IllegalArgumentException expected but not thrown");
17930 } catch (final IllegalArgumentException ignore) {
17931 }
17932 assertNull(calibrator);
17933 }
17934
17935 @Test
17936 public void testConstructor155() throws WrongSizeException {
17937 final Collection<StandardDeviationBodyKinematics> measurements =
17938 Collections.emptyList();
17939
17940 final Matrix ba = generateBa();
17941 final double[] bias = ba.getBuffer();
17942 final double biasX = ba.getElementAtIndex(0);
17943 final double biasY = ba.getElementAtIndex(1);
17944 final double biasZ = ba.getElementAtIndex(2);
17945
17946 final Matrix ma = generateMaCommonAxis();
17947 final double sx = ma.getElementAt(0, 0);
17948 final double sy = ma.getElementAt(1, 1);
17949 final double sz = ma.getElementAt(2, 2);
17950 final double mxy = ma.getElementAt(0, 1);
17951 final double mxz = ma.getElementAt(0, 2);
17952 final double myx = ma.getElementAt(1, 0);
17953 final double myz = ma.getElementAt(1, 2);
17954 final double mzx = ma.getElementAt(2, 0);
17955 final double mzy = ma.getElementAt(2, 1);
17956
17957 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17958 final double latitude = Math.toRadians(
17959 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17960 final double longitude = Math.toRadians(
17961 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17962 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17963 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17964 final NEDVelocity nedVelocity = new NEDVelocity();
17965 final ECEFPosition ecefPosition = new ECEFPosition();
17966 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17967 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17968 ecefPosition, ecefVelocity);
17969 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17970 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17971 final double gravityNorm = gravity.getNorm();
17972
17973 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17974 new KnownBiasAndGravityNormAccelerometerCalibrator(
17975 gravityNorm, measurements,
17976 true, ba, ma);
17977
17978
17979 assertEquals(calibrator.getBiasX(), biasX, 0.0);
17980 assertEquals(calibrator.getBiasY(), biasY, 0.0);
17981 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17982 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17983 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17984 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17985 final Acceleration bx2 = new Acceleration(0.0,
17986 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17987 calibrator.getBiasXAsAcceleration(bx2);
17988 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17989 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17990 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17991 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17992 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17993 final Acceleration by2 = new Acceleration(0.0,
17994 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17995 calibrator.getBiasYAsAcceleration(by2);
17996 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17997 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17998 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17999 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18000 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18001 final Acceleration bz2 = new Acceleration(0.0,
18002 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18003 calibrator.getBiasZAsAcceleration(bz2);
18004 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18005 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18006 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18007 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18008 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18009 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18010 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18011 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18012 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18013 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18014 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18015 final double[] bias1 = calibrator.getBias();
18016 assertArrayEquals(bias1, bias, 0.0);
18017 final double[] bias2 = new double[3];
18018 calibrator.getBias(bias2);
18019 assertArrayEquals(bias1, bias2, 0.0);
18020 final Matrix b1 = calibrator.getBiasAsMatrix();
18021 assertEquals(b1, ba);
18022 final Matrix b2 = new Matrix(3, 1);
18023 calibrator.getBiasAsMatrix(b2);
18024 assertEquals(b1, b2);
18025 final Matrix ma1 = new Matrix(3, 3);
18026 ma1.setSubmatrix(0, 0,
18027 2, 2,
18028 new double[]{sx, myx, mzx,
18029 mxy, sy, mzy,
18030 mxz, myz, sz});
18031 assertEquals(calibrator.getInitialMa(), ma1);
18032 final Matrix ma2 = new Matrix(3, 3);
18033 calibrator.getInitialMa(ma2);
18034 assertEquals(ma1, ma2);
18035 assertSame(calibrator.getMeasurements(), measurements);
18036 assertTrue(calibrator.isCommonAxisUsed());
18037 assertNull(calibrator.getListener());
18038 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18039 assertFalse(calibrator.isReady());
18040 assertFalse(calibrator.isRunning());
18041 assertNull(calibrator.getEstimatedMa());
18042 assertNull(calibrator.getEstimatedSx());
18043 assertNull(calibrator.getEstimatedSy());
18044 assertNull(calibrator.getEstimatedSz());
18045 assertNull(calibrator.getEstimatedMxy());
18046 assertNull(calibrator.getEstimatedMxz());
18047 assertNull(calibrator.getEstimatedMyx());
18048 assertNull(calibrator.getEstimatedMyz());
18049 assertNull(calibrator.getEstimatedMzx());
18050 assertNull(calibrator.getEstimatedMzy());
18051 assertNull(calibrator.getEstimatedCovariance());
18052 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18053 assertNotNull(calibrator.getGroundTruthGravityNorm());
18054 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18055 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18056 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18057 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18058 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18059 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18060 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18061
18062
18063 calibrator = null;
18064 try {
18065 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18066 -gravityNorm, measurements,
18067 true, ba, ma);
18068 fail("IllegalArgumentException expected but not thrown");
18069 } catch (final IllegalArgumentException ignore) {
18070 }
18071 try {
18072 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18073 gravityNorm, measurements, true,
18074 new Matrix(1, 1), ma);
18075 fail("IllegalArgumentException expected but not thrown");
18076 } catch (final IllegalArgumentException ignore) {
18077 }
18078 try {
18079 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18080 gravityNorm, measurements, true,
18081 new Matrix(1, 3), ma);
18082 fail("IllegalArgumentException expected but not thrown");
18083 } catch (final IllegalArgumentException ignore) {
18084 }
18085 try {
18086 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18087 gravityNorm, measurements, true,
18088 ba, new Matrix(1, 3));
18089 fail("IllegalArgumentException expected but not thrown");
18090 } catch (final IllegalArgumentException ignore) {
18091 }
18092 try {
18093 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18094 gravityNorm, measurements, true,
18095 ba, new Matrix(3, 1));
18096 fail("IllegalArgumentException expected but not thrown");
18097 } catch (final IllegalArgumentException ignore) {
18098 }
18099 assertNull(calibrator);
18100 }
18101
18102 @Test
18103 public void testConstructor156() throws WrongSizeException {
18104 final Collection<StandardDeviationBodyKinematics> measurements =
18105 Collections.emptyList();
18106
18107 final Matrix ba = generateBa();
18108 final double[] bias = ba.getBuffer();
18109 final double biasX = ba.getElementAtIndex(0);
18110 final double biasY = ba.getElementAtIndex(1);
18111 final double biasZ = ba.getElementAtIndex(2);
18112
18113 final Matrix ma = generateMaCommonAxis();
18114 final double sx = ma.getElementAt(0, 0);
18115 final double sy = ma.getElementAt(1, 1);
18116 final double sz = ma.getElementAt(2, 2);
18117 final double mxy = ma.getElementAt(0, 1);
18118 final double mxz = ma.getElementAt(0, 2);
18119 final double myx = ma.getElementAt(1, 0);
18120 final double myz = ma.getElementAt(1, 2);
18121 final double mzx = ma.getElementAt(2, 0);
18122 final double mzy = ma.getElementAt(2, 1);
18123
18124 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18125 final double latitude = Math.toRadians(
18126 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18127 final double longitude = Math.toRadians(
18128 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18129 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18130 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18131 final NEDVelocity nedVelocity = new NEDVelocity();
18132 final ECEFPosition ecefPosition = new ECEFPosition();
18133 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18134 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18135 ecefPosition, ecefVelocity);
18136 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18137 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18138 final double gravityNorm = gravity.getNorm();
18139
18140 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18141 new KnownBiasAndGravityNormAccelerometerCalibrator(
18142 gravityNorm, measurements,
18143 true, ba, ma, this);
18144
18145
18146 assertEquals(calibrator.getBiasX(), biasX, 0.0);
18147 assertEquals(calibrator.getBiasY(), biasY, 0.0);
18148 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
18149 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18150 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18151 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18152 final Acceleration bx2 = new Acceleration(0.0,
18153 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18154 calibrator.getBiasXAsAcceleration(bx2);
18155 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18156 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18157 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18158 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18159 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18160 final Acceleration by2 = new Acceleration(0.0,
18161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18162 calibrator.getBiasYAsAcceleration(by2);
18163 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18164 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18165 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18166 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18167 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18168 final Acceleration bz2 = new Acceleration(0.0,
18169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18170 calibrator.getBiasZAsAcceleration(bz2);
18171 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18172 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18173 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18174 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18175 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18176 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18177 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18178 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18179 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18180 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18181 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18182 final double[] bias1 = calibrator.getBias();
18183 assertArrayEquals(bias1, bias, 0.0);
18184 final double[] bias2 = new double[3];
18185 calibrator.getBias(bias2);
18186 assertArrayEquals(bias1, bias2, 0.0);
18187 final Matrix b1 = calibrator.getBiasAsMatrix();
18188 assertEquals(b1, ba);
18189 final Matrix b2 = new Matrix(3, 1);
18190 calibrator.getBiasAsMatrix(b2);
18191 assertEquals(b1, b2);
18192 final Matrix ma1 = new Matrix(3, 3);
18193 ma1.setSubmatrix(0, 0,
18194 2, 2,
18195 new double[]{sx, myx, mzx,
18196 mxy, sy, mzy,
18197 mxz, myz, sz});
18198 assertEquals(calibrator.getInitialMa(), ma1);
18199 final Matrix ma2 = new Matrix(3, 3);
18200 calibrator.getInitialMa(ma2);
18201 assertEquals(ma1, ma2);
18202 assertSame(calibrator.getMeasurements(), measurements);
18203 assertTrue(calibrator.isCommonAxisUsed());
18204 assertSame(calibrator.getListener(), this);
18205 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18206 assertFalse(calibrator.isReady());
18207 assertFalse(calibrator.isRunning());
18208 assertNull(calibrator.getEstimatedMa());
18209 assertNull(calibrator.getEstimatedSx());
18210 assertNull(calibrator.getEstimatedSy());
18211 assertNull(calibrator.getEstimatedSz());
18212 assertNull(calibrator.getEstimatedMxy());
18213 assertNull(calibrator.getEstimatedMxz());
18214 assertNull(calibrator.getEstimatedMyx());
18215 assertNull(calibrator.getEstimatedMyz());
18216 assertNull(calibrator.getEstimatedMzx());
18217 assertNull(calibrator.getEstimatedMzy());
18218 assertNull(calibrator.getEstimatedCovariance());
18219 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18220 assertNotNull(calibrator.getGroundTruthGravityNorm());
18221 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18222 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18223 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18224 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18225 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18226 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18227 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18228
18229
18230 calibrator = null;
18231 try {
18232 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18233 -gravityNorm, measurements,
18234 true, ba, ma, this);
18235 fail("IllegalArgumentException expected but not thrown");
18236 } catch (final IllegalArgumentException ignore) {
18237 }
18238 try {
18239 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18240 gravityNorm, measurements, true,
18241 new Matrix(1, 1), ma, this);
18242 fail("IllegalArgumentException expected but not thrown");
18243 } catch (final IllegalArgumentException ignore) {
18244 }
18245 try {
18246 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18247 gravityNorm, measurements, true,
18248 new Matrix(1, 3), ma, this);
18249 fail("IllegalArgumentException expected but not thrown");
18250 } catch (final IllegalArgumentException ignore) {
18251 }
18252 try {
18253 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18254 gravityNorm, measurements, true,
18255 ba, new Matrix(1, 3), this);
18256 fail("IllegalArgumentException expected but not thrown");
18257 } catch (final IllegalArgumentException ignore) {
18258 }
18259 try {
18260 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18261 gravityNorm, measurements, true,
18262 ba, new Matrix(3, 1), this);
18263 fail("IllegalArgumentException expected but not thrown");
18264 } catch (final IllegalArgumentException ignore) {
18265 }
18266 assertNull(calibrator);
18267 }
18268
18269 @Test
18270 public void testConstructor157() throws WrongSizeException {
18271 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18272 final double latitude = Math.toRadians(
18273 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18274 final double longitude = Math.toRadians(
18275 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18276 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18277 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18278 final NEDVelocity nedVelocity = new NEDVelocity();
18279 final ECEFPosition ecefPosition = new ECEFPosition();
18280 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18281 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18282 ecefPosition, ecefVelocity);
18283 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18284 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18285 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18286
18287 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18288 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
18289
18290
18291 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18292 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18293 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18294 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18295 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18296 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18297 final Acceleration bx2 = new Acceleration(0.0,
18298 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18299 calibrator.getBiasXAsAcceleration(bx2);
18300 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18301 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18302 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18303 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18304 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18305 final Acceleration by2 = new Acceleration(0.0,
18306 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18307 calibrator.getBiasYAsAcceleration(by2);
18308 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18309 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18310 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18311 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18312 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18313 final Acceleration bz2 = new Acceleration(0.0,
18314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18315 calibrator.getBiasZAsAcceleration(bz2);
18316 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18317 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18318 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18319 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18320 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18321 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18322 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18323 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18324 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18325 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18326 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18327 final double[] bias1 = calibrator.getBias();
18328 assertArrayEquals(bias1, new double[3], 0.0);
18329 final double[] bias2 = new double[3];
18330 calibrator.getBias(bias2);
18331 assertArrayEquals(bias1, bias2, 0.0);
18332 final Matrix b1 = calibrator.getBiasAsMatrix();
18333 assertEquals(b1, new Matrix(3, 1));
18334 final Matrix b2 = new Matrix(3, 1);
18335 calibrator.getBiasAsMatrix(b2);
18336 assertEquals(b1, b2);
18337 final Matrix ma1 = calibrator.getInitialMa();
18338 assertEquals(ma1, new Matrix(3, 3));
18339 final Matrix ma2 = new Matrix(3, 3);
18340 calibrator.getInitialMa(ma2);
18341 assertEquals(ma1, ma2);
18342 assertNull(calibrator.getMeasurements());
18343 assertFalse(calibrator.isCommonAxisUsed());
18344 assertNull(calibrator.getListener());
18345 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18346 assertFalse(calibrator.isReady());
18347 assertFalse(calibrator.isRunning());
18348 assertNull(calibrator.getEstimatedMa());
18349 assertNull(calibrator.getEstimatedSx());
18350 assertNull(calibrator.getEstimatedSy());
18351 assertNull(calibrator.getEstimatedSz());
18352 assertNull(calibrator.getEstimatedMxy());
18353 assertNull(calibrator.getEstimatedMxz());
18354 assertNull(calibrator.getEstimatedMyx());
18355 assertNull(calibrator.getEstimatedMyz());
18356 assertNull(calibrator.getEstimatedMzx());
18357 assertNull(calibrator.getEstimatedMzy());
18358 assertNull(calibrator.getEstimatedCovariance());
18359 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18360 assertNotNull(calibrator.getGroundTruthGravityNorm());
18361 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18362 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18363 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18364 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18365 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18366 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18367 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18368
18369
18370 final Acceleration invalidGravityNorm = new Acceleration(
18371 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18372
18373 calibrator = null;
18374 try {
18375 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18376 invalidGravityNorm);
18377 fail("IllegalArgumentException expected but not thrown");
18378 } catch (final IllegalArgumentException ignore) {
18379 }
18380 assertNull(calibrator);
18381 }
18382
18383 @Test
18384 public void testConstructor158() throws WrongSizeException {
18385 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18386 final double latitude = Math.toRadians(
18387 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18388 final double longitude = Math.toRadians(
18389 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18390 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18391 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18392 final NEDVelocity nedVelocity = new NEDVelocity();
18393 final ECEFPosition ecefPosition = new ECEFPosition();
18394 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18395 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18396 ecefPosition, ecefVelocity);
18397 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18398 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18399 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18400
18401 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18402 new KnownBiasAndGravityNormAccelerometerCalibrator(
18403 gravityNorm, this);
18404
18405
18406 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18407 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18408 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18409 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18410 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18411 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18412 final Acceleration bx2 = new Acceleration(0.0,
18413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18414 calibrator.getBiasXAsAcceleration(bx2);
18415 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18416 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18417 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18418 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18419 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18420 final Acceleration by2 = new Acceleration(0.0,
18421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18422 calibrator.getBiasYAsAcceleration(by2);
18423 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18424 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18425 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18426 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18427 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18428 final Acceleration bz2 = new Acceleration(0.0,
18429 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18430 calibrator.getBiasZAsAcceleration(bz2);
18431 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18432 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18433 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18434 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18435 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18436 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18437 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18438 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18439 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18440 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18441 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18442 final double[] bias1 = calibrator.getBias();
18443 assertArrayEquals(bias1, new double[3], 0.0);
18444 final double[] bias2 = new double[3];
18445 calibrator.getBias(bias2);
18446 assertArrayEquals(bias1, bias2, 0.0);
18447 final Matrix b1 = calibrator.getBiasAsMatrix();
18448 assertEquals(b1, new Matrix(3, 1));
18449 final Matrix b2 = new Matrix(3, 1);
18450 calibrator.getBiasAsMatrix(b2);
18451 assertEquals(b1, b2);
18452 final Matrix ma1 = calibrator.getInitialMa();
18453 assertEquals(ma1, new Matrix(3, 3));
18454 final Matrix ma2 = new Matrix(3, 3);
18455 calibrator.getInitialMa(ma2);
18456 assertEquals(ma1, ma2);
18457 assertNull(calibrator.getMeasurements());
18458 assertFalse(calibrator.isCommonAxisUsed());
18459 assertSame(calibrator.getListener(), this);
18460 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18461 assertFalse(calibrator.isReady());
18462 assertFalse(calibrator.isRunning());
18463 assertNull(calibrator.getEstimatedMa());
18464 assertNull(calibrator.getEstimatedSx());
18465 assertNull(calibrator.getEstimatedSy());
18466 assertNull(calibrator.getEstimatedSz());
18467 assertNull(calibrator.getEstimatedMxy());
18468 assertNull(calibrator.getEstimatedMxz());
18469 assertNull(calibrator.getEstimatedMyx());
18470 assertNull(calibrator.getEstimatedMyz());
18471 assertNull(calibrator.getEstimatedMzx());
18472 assertNull(calibrator.getEstimatedMzy());
18473 assertNull(calibrator.getEstimatedCovariance());
18474 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18475 assertNotNull(calibrator.getGroundTruthGravityNorm());
18476 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18477 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18478 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18479 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18480 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18481 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18482 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18483
18484
18485 final Acceleration invalidGravityNorm = new Acceleration(
18486 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18487
18488 calibrator = null;
18489 try {
18490 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18491 invalidGravityNorm, this);
18492 fail("IllegalArgumentException expected but not thrown");
18493 } catch (final IllegalArgumentException ignore) {
18494 }
18495 assertNull(calibrator);
18496 }
18497
18498 @Test
18499 public void testConstructor159() throws WrongSizeException {
18500 final Collection<StandardDeviationBodyKinematics> measurements =
18501 Collections.emptyList();
18502
18503 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18504 final double latitude = Math.toRadians(
18505 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18506 final double longitude = Math.toRadians(
18507 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18508 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18509 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18510 final NEDVelocity nedVelocity = new NEDVelocity();
18511 final ECEFPosition ecefPosition = new ECEFPosition();
18512 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18513 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18514 ecefPosition, ecefVelocity);
18515 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18516 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18517 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18518
18519 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18520 new KnownBiasAndGravityNormAccelerometerCalibrator(
18521 gravityNorm, measurements);
18522
18523
18524 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18525 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18526 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18527 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18528 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18529 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530 final Acceleration bx2 = new Acceleration(0.0,
18531 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532 calibrator.getBiasXAsAcceleration(bx2);
18533 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18534 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18536 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18537 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18538 final Acceleration by2 = new Acceleration(0.0,
18539 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18540 calibrator.getBiasYAsAcceleration(by2);
18541 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18542 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18543 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18544 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18545 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18546 final Acceleration bz2 = new Acceleration(0.0,
18547 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548 calibrator.getBiasZAsAcceleration(bz2);
18549 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18550 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18551 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18552 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18553 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18554 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18555 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18556 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18557 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18558 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18559 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18560 final double[] bias1 = calibrator.getBias();
18561 assertArrayEquals(bias1, new double[3], 0.0);
18562 final double[] bias2 = new double[3];
18563 calibrator.getBias(bias2);
18564 assertArrayEquals(bias1, bias2, 0.0);
18565 final Matrix b1 = calibrator.getBiasAsMatrix();
18566 assertEquals(b1, new Matrix(3, 1));
18567 final Matrix b2 = new Matrix(3, 1);
18568 calibrator.getBiasAsMatrix(b2);
18569 assertEquals(b1, b2);
18570 final Matrix ma1 = calibrator.getInitialMa();
18571 assertEquals(ma1, new Matrix(3, 3));
18572 final Matrix ma2 = new Matrix(3, 3);
18573 calibrator.getInitialMa(ma2);
18574 assertEquals(ma1, ma2);
18575 assertSame(calibrator.getMeasurements(), measurements);
18576 assertFalse(calibrator.isCommonAxisUsed());
18577 assertNull(calibrator.getListener());
18578 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18579 assertFalse(calibrator.isReady());
18580 assertFalse(calibrator.isRunning());
18581 assertNull(calibrator.getEstimatedMa());
18582 assertNull(calibrator.getEstimatedSx());
18583 assertNull(calibrator.getEstimatedSy());
18584 assertNull(calibrator.getEstimatedSz());
18585 assertNull(calibrator.getEstimatedMxy());
18586 assertNull(calibrator.getEstimatedMxz());
18587 assertNull(calibrator.getEstimatedMyx());
18588 assertNull(calibrator.getEstimatedMyz());
18589 assertNull(calibrator.getEstimatedMzx());
18590 assertNull(calibrator.getEstimatedMzy());
18591 assertNull(calibrator.getEstimatedCovariance());
18592 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18593 assertNotNull(calibrator.getGroundTruthGravityNorm());
18594 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18595 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18596 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18597 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18598 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18599 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18600 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18601
18602
18603 final Acceleration invalidGravityNorm = new Acceleration(
18604 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18605
18606 calibrator = null;
18607 try {
18608 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18609 invalidGravityNorm, measurements);
18610 fail("IllegalArgumentException expected but not thrown");
18611 } catch (final IllegalArgumentException ignore) {
18612 }
18613 assertNull(calibrator);
18614 }
18615
18616 @Test
18617 public void testConstructor160() throws WrongSizeException {
18618 final Collection<StandardDeviationBodyKinematics> measurements =
18619 Collections.emptyList();
18620
18621 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18622 final double latitude = Math.toRadians(
18623 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18624 final double longitude = Math.toRadians(
18625 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18626 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18627 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18628 final NEDVelocity nedVelocity = new NEDVelocity();
18629 final ECEFPosition ecefPosition = new ECEFPosition();
18630 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18631 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18632 ecefPosition, ecefVelocity);
18633 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18634 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18635 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18636
18637 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18638 new KnownBiasAndGravityNormAccelerometerCalibrator(
18639 gravityNorm, measurements, this);
18640
18641
18642 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18643 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18644 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18645 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18646 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18647 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18648 final Acceleration bx2 = new Acceleration(0.0,
18649 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18650 calibrator.getBiasXAsAcceleration(bx2);
18651 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18652 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18653 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18654 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18655 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18656 final Acceleration by2 = new Acceleration(0.0,
18657 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18658 calibrator.getBiasYAsAcceleration(by2);
18659 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18660 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18661 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18662 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18663 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18664 final Acceleration bz2 = new Acceleration(0.0,
18665 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18666 calibrator.getBiasZAsAcceleration(bz2);
18667 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18668 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18669 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18670 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18671 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18672 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18673 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18674 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18675 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18676 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18677 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18678 final double[] bias1 = calibrator.getBias();
18679 assertArrayEquals(bias1, new double[3], 0.0);
18680 final double[] bias2 = new double[3];
18681 calibrator.getBias(bias2);
18682 assertArrayEquals(bias1, bias2, 0.0);
18683 final Matrix b1 = calibrator.getBiasAsMatrix();
18684 assertEquals(b1, new Matrix(3, 1));
18685 final Matrix b2 = new Matrix(3, 1);
18686 calibrator.getBiasAsMatrix(b2);
18687 assertEquals(b1, b2);
18688 final Matrix ma1 = calibrator.getInitialMa();
18689 assertEquals(ma1, new Matrix(3, 3));
18690 final Matrix ma2 = new Matrix(3, 3);
18691 calibrator.getInitialMa(ma2);
18692 assertEquals(ma1, ma2);
18693 assertSame(calibrator.getMeasurements(), measurements);
18694 assertFalse(calibrator.isCommonAxisUsed());
18695 assertSame(calibrator.getListener(), this);
18696 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18697 assertFalse(calibrator.isReady());
18698 assertFalse(calibrator.isRunning());
18699 assertNull(calibrator.getEstimatedMa());
18700 assertNull(calibrator.getEstimatedSx());
18701 assertNull(calibrator.getEstimatedSy());
18702 assertNull(calibrator.getEstimatedSz());
18703 assertNull(calibrator.getEstimatedMxy());
18704 assertNull(calibrator.getEstimatedMxz());
18705 assertNull(calibrator.getEstimatedMyx());
18706 assertNull(calibrator.getEstimatedMyz());
18707 assertNull(calibrator.getEstimatedMzx());
18708 assertNull(calibrator.getEstimatedMzy());
18709 assertNull(calibrator.getEstimatedCovariance());
18710 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18711 assertNotNull(calibrator.getGroundTruthGravityNorm());
18712 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18713 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18714 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18715 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18716 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18717 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18718 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18719
18720
18721 final Acceleration invalidGravityNorm = new Acceleration(
18722 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18723
18724 calibrator = null;
18725 try {
18726 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18727 invalidGravityNorm, measurements, this);
18728 fail("IllegalArgumentException expected but not thrown");
18729 } catch (final IllegalArgumentException ignore) {
18730 }
18731 assertNull(calibrator);
18732 }
18733
18734 @Test
18735 public void testConstructor161() throws WrongSizeException {
18736 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18737 final double latitude = Math.toRadians(
18738 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18739 final double longitude = Math.toRadians(
18740 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18741 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18742 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18743 final NEDVelocity nedVelocity = new NEDVelocity();
18744 final ECEFPosition ecefPosition = new ECEFPosition();
18745 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18746 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18747 ecefPosition, ecefVelocity);
18748 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18749 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18750 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18751
18752 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18753 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
18754 true);
18755
18756
18757 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18758 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18759 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18760 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18761 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18762 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18763 final Acceleration bx2 = new Acceleration(0.0,
18764 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18765 calibrator.getBiasXAsAcceleration(bx2);
18766 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18767 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18768 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18769 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18770 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18771 final Acceleration by2 = new Acceleration(0.0,
18772 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18773 calibrator.getBiasYAsAcceleration(by2);
18774 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18775 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18776 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18777 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18778 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18779 final Acceleration bz2 = new Acceleration(0.0,
18780 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18781 calibrator.getBiasZAsAcceleration(bz2);
18782 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18783 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18784 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18785 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18786 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18787 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18788 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18789 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18790 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18791 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18792 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18793 final double[] bias1 = calibrator.getBias();
18794 assertArrayEquals(bias1, new double[3], 0.0);
18795 final double[] bias2 = new double[3];
18796 calibrator.getBias(bias2);
18797 assertArrayEquals(bias1, bias2, 0.0);
18798 final Matrix b1 = calibrator.getBiasAsMatrix();
18799 assertEquals(b1, new Matrix(3, 1));
18800 final Matrix b2 = new Matrix(3, 1);
18801 calibrator.getBiasAsMatrix(b2);
18802 assertEquals(b1, b2);
18803 final Matrix ma1 = calibrator.getInitialMa();
18804 assertEquals(ma1, new Matrix(3, 3));
18805 final Matrix ma2 = new Matrix(3, 3);
18806 calibrator.getInitialMa(ma2);
18807 assertEquals(ma1, ma2);
18808 assertNull(calibrator.getMeasurements());
18809 assertTrue(calibrator.isCommonAxisUsed());
18810 assertNull(calibrator.getListener());
18811 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18812 assertFalse(calibrator.isReady());
18813 assertFalse(calibrator.isRunning());
18814 assertNull(calibrator.getEstimatedMa());
18815 assertNull(calibrator.getEstimatedSx());
18816 assertNull(calibrator.getEstimatedSy());
18817 assertNull(calibrator.getEstimatedSz());
18818 assertNull(calibrator.getEstimatedMxy());
18819 assertNull(calibrator.getEstimatedMxz());
18820 assertNull(calibrator.getEstimatedMyx());
18821 assertNull(calibrator.getEstimatedMyz());
18822 assertNull(calibrator.getEstimatedMzx());
18823 assertNull(calibrator.getEstimatedMzy());
18824 assertNull(calibrator.getEstimatedCovariance());
18825 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18826 assertNotNull(calibrator.getGroundTruthGravityNorm());
18827 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18828 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18829 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18830 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18831 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18832 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18833 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18834
18835
18836 final Acceleration invalidGravityNorm = new Acceleration(
18837 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18838
18839 calibrator = null;
18840 try {
18841 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18842 invalidGravityNorm, true);
18843 fail("IllegalArgumentException expected but not thrown");
18844 } catch (final IllegalArgumentException ignore) {
18845 }
18846 assertNull(calibrator);
18847 }
18848
18849 @Test
18850 public void testConstructor162() throws WrongSizeException {
18851 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18852 final double latitude = Math.toRadians(
18853 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18854 final double longitude = Math.toRadians(
18855 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18856 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18857 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18858 final NEDVelocity nedVelocity = new NEDVelocity();
18859 final ECEFPosition ecefPosition = new ECEFPosition();
18860 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18861 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18862 ecefPosition, ecefVelocity);
18863 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18864 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18865 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18866
18867 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18868 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
18869 true, this);
18870
18871
18872 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18873 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18874 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18875 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18876 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18877 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18878 final Acceleration bx2 = new Acceleration(0.0,
18879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18880 calibrator.getBiasXAsAcceleration(bx2);
18881 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18882 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18883 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18884 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18885 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18886 final Acceleration by2 = new Acceleration(0.0,
18887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18888 calibrator.getBiasYAsAcceleration(by2);
18889 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18890 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18891 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18892 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18893 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18894 final Acceleration bz2 = new Acceleration(0.0,
18895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18896 calibrator.getBiasZAsAcceleration(bz2);
18897 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18898 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18899 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18900 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18901 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18902 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18903 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18904 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18905 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18906 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18907 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18908 final double[] bias1 = calibrator.getBias();
18909 assertArrayEquals(bias1, new double[3], 0.0);
18910 final double[] bias2 = new double[3];
18911 calibrator.getBias(bias2);
18912 assertArrayEquals(bias1, bias2, 0.0);
18913 final Matrix b1 = calibrator.getBiasAsMatrix();
18914 assertEquals(b1, new Matrix(3, 1));
18915 final Matrix b2 = new Matrix(3, 1);
18916 calibrator.getBiasAsMatrix(b2);
18917 assertEquals(b1, b2);
18918 final Matrix ma1 = calibrator.getInitialMa();
18919 assertEquals(ma1, new Matrix(3, 3));
18920 final Matrix ma2 = new Matrix(3, 3);
18921 calibrator.getInitialMa(ma2);
18922 assertEquals(ma1, ma2);
18923 assertNull(calibrator.getMeasurements());
18924 assertTrue(calibrator.isCommonAxisUsed());
18925 assertSame(calibrator.getListener(), this);
18926 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18927 assertFalse(calibrator.isReady());
18928 assertFalse(calibrator.isRunning());
18929 assertNull(calibrator.getEstimatedMa());
18930 assertNull(calibrator.getEstimatedSx());
18931 assertNull(calibrator.getEstimatedSy());
18932 assertNull(calibrator.getEstimatedSz());
18933 assertNull(calibrator.getEstimatedMxy());
18934 assertNull(calibrator.getEstimatedMxz());
18935 assertNull(calibrator.getEstimatedMyx());
18936 assertNull(calibrator.getEstimatedMyz());
18937 assertNull(calibrator.getEstimatedMzx());
18938 assertNull(calibrator.getEstimatedMzy());
18939 assertNull(calibrator.getEstimatedCovariance());
18940 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18941 assertNotNull(calibrator.getGroundTruthGravityNorm());
18942 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18943 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18944 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18945 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18946 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18947 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18948 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18949
18950
18951 final Acceleration invalidGravityNorm = new Acceleration(
18952 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18953
18954 calibrator = null;
18955 try {
18956 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18957 invalidGravityNorm, true, this);
18958 fail("IllegalArgumentException expected but not thrown");
18959 } catch (final IllegalArgumentException ignore) {
18960 }
18961 assertNull(calibrator);
18962 }
18963
18964 @Test
18965 public void testConstructor163() throws WrongSizeException {
18966 final Collection<StandardDeviationBodyKinematics> measurements =
18967 Collections.emptyList();
18968 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18969 final double latitude = Math.toRadians(
18970 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18971 final double longitude = Math.toRadians(
18972 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18973 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18974 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18975 final NEDVelocity nedVelocity = new NEDVelocity();
18976 final ECEFPosition ecefPosition = new ECEFPosition();
18977 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18978 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18979 ecefPosition, ecefVelocity);
18980 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18981 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18982 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18983
18984 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18985 new KnownBiasAndGravityNormAccelerometerCalibrator(
18986 gravityNorm, measurements,
18987 true);
18988
18989
18990 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18991 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18992 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18993 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18994 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18995 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18996 final Acceleration bx2 = new Acceleration(0.0,
18997 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18998 calibrator.getBiasXAsAcceleration(bx2);
18999 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19000 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19001 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19002 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19003 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19004 final Acceleration by2 = new Acceleration(0.0,
19005 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19006 calibrator.getBiasYAsAcceleration(by2);
19007 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19008 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19009 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19010 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19011 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19012 final Acceleration bz2 = new Acceleration(0.0,
19013 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19014 calibrator.getBiasZAsAcceleration(bz2);
19015 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19016 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19017 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19018 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19019 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19020 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19021 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19022 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19023 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19024 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19025 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19026 final double[] bias1 = calibrator.getBias();
19027 assertArrayEquals(bias1, new double[3], 0.0);
19028 final double[] bias2 = new double[3];
19029 calibrator.getBias(bias2);
19030 assertArrayEquals(bias1, bias2, 0.0);
19031 final Matrix b1 = calibrator.getBiasAsMatrix();
19032 assertEquals(b1, new Matrix(3, 1));
19033 final Matrix b2 = new Matrix(3, 1);
19034 calibrator.getBiasAsMatrix(b2);
19035 assertEquals(b1, b2);
19036 final Matrix ma1 = calibrator.getInitialMa();
19037 assertEquals(ma1, new Matrix(3, 3));
19038 final Matrix ma2 = new Matrix(3, 3);
19039 calibrator.getInitialMa(ma2);
19040 assertEquals(ma1, ma2);
19041 assertSame(calibrator.getMeasurements(), measurements);
19042 assertTrue(calibrator.isCommonAxisUsed());
19043 assertNull(calibrator.getListener());
19044 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19045 assertFalse(calibrator.isReady());
19046 assertFalse(calibrator.isRunning());
19047 assertNull(calibrator.getEstimatedMa());
19048 assertNull(calibrator.getEstimatedSx());
19049 assertNull(calibrator.getEstimatedSy());
19050 assertNull(calibrator.getEstimatedSz());
19051 assertNull(calibrator.getEstimatedMxy());
19052 assertNull(calibrator.getEstimatedMxz());
19053 assertNull(calibrator.getEstimatedMyx());
19054 assertNull(calibrator.getEstimatedMyz());
19055 assertNull(calibrator.getEstimatedMzx());
19056 assertNull(calibrator.getEstimatedMzy());
19057 assertNull(calibrator.getEstimatedCovariance());
19058 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19059 assertNotNull(calibrator.getGroundTruthGravityNorm());
19060 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19061 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19062 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19063 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19064 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19065 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19066 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19067
19068
19069 final Acceleration invalidGravityNorm = new Acceleration(
19070 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19071
19072 calibrator = null;
19073 try {
19074 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19075 invalidGravityNorm, measurements,
19076 true);
19077 fail("IllegalArgumentException expected but not thrown");
19078 } catch (final IllegalArgumentException ignore) {
19079 }
19080 assertNull(calibrator);
19081 }
19082
19083 @Test
19084 public void testConstructor164() throws WrongSizeException {
19085 final Collection<StandardDeviationBodyKinematics> measurements =
19086 Collections.emptyList();
19087 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19088 final double latitude = Math.toRadians(
19089 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19090 final double longitude = Math.toRadians(
19091 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19092 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19093 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19094 final NEDVelocity nedVelocity = new NEDVelocity();
19095 final ECEFPosition ecefPosition = new ECEFPosition();
19096 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19097 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19098 ecefPosition, ecefVelocity);
19099 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19100 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19101 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19102
19103 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19104 new KnownBiasAndGravityNormAccelerometerCalibrator(
19105 gravityNorm, measurements,
19106 true, this);
19107
19108
19109 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
19110 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
19111 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
19112 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19113 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19114 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19115 final Acceleration bx2 = new Acceleration(0.0,
19116 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19117 calibrator.getBiasXAsAcceleration(bx2);
19118 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19119 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19120 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19121 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19122 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19123 final Acceleration by2 = new Acceleration(0.0,
19124 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19125 calibrator.getBiasYAsAcceleration(by2);
19126 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19127 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19128 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19129 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19130 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19131 final Acceleration bz2 = new Acceleration(0.0,
19132 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19133 calibrator.getBiasZAsAcceleration(bz2);
19134 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19135 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19136 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19137 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19138 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19139 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19140 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19141 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19142 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19143 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19144 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19145 final double[] bias1 = calibrator.getBias();
19146 assertArrayEquals(bias1, new double[3], 0.0);
19147 final double[] bias2 = new double[3];
19148 calibrator.getBias(bias2);
19149 assertArrayEquals(bias1, bias2, 0.0);
19150 final Matrix b1 = calibrator.getBiasAsMatrix();
19151 assertEquals(b1, new Matrix(3, 1));
19152 final Matrix b2 = new Matrix(3, 1);
19153 calibrator.getBiasAsMatrix(b2);
19154 assertEquals(b1, b2);
19155 final Matrix ma1 = calibrator.getInitialMa();
19156 assertEquals(ma1, new Matrix(3, 3));
19157 final Matrix ma2 = new Matrix(3, 3);
19158 calibrator.getInitialMa(ma2);
19159 assertEquals(ma1, ma2);
19160 assertSame(calibrator.getMeasurements(), measurements);
19161 assertTrue(calibrator.isCommonAxisUsed());
19162 assertSame(calibrator.getListener(), this);
19163 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19164 assertFalse(calibrator.isReady());
19165 assertFalse(calibrator.isRunning());
19166 assertNull(calibrator.getEstimatedMa());
19167 assertNull(calibrator.getEstimatedSx());
19168 assertNull(calibrator.getEstimatedSy());
19169 assertNull(calibrator.getEstimatedSz());
19170 assertNull(calibrator.getEstimatedMxy());
19171 assertNull(calibrator.getEstimatedMxz());
19172 assertNull(calibrator.getEstimatedMyx());
19173 assertNull(calibrator.getEstimatedMyz());
19174 assertNull(calibrator.getEstimatedMzx());
19175 assertNull(calibrator.getEstimatedMzy());
19176 assertNull(calibrator.getEstimatedCovariance());
19177 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19178 assertNotNull(calibrator.getGroundTruthGravityNorm());
19179 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19180 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19181 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19182 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19183 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19184 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19185 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19186
19187
19188 final Acceleration invalidGravityNorm = new Acceleration(
19189 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19190
19191 calibrator = null;
19192 try {
19193 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19194 invalidGravityNorm, measurements,
19195 true, this);
19196 fail("IllegalArgumentException expected but not thrown");
19197 } catch (final IllegalArgumentException ignore) {
19198 }
19199 assertNull(calibrator);
19200 }
19201
19202 @Test
19203 public void testConstructor165() throws WrongSizeException {
19204 final Matrix ba = generateBa();
19205 final double biasX = ba.getElementAtIndex(0);
19206 final double biasY = ba.getElementAtIndex(1);
19207 final double biasZ = ba.getElementAtIndex(2);
19208
19209 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19210 final double latitude = Math.toRadians(
19211 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19212 final double longitude = Math.toRadians(
19213 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19214 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19215 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19216 final NEDVelocity nedVelocity = new NEDVelocity();
19217 final ECEFPosition ecefPosition = new ECEFPosition();
19218 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19219 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19220 ecefPosition, ecefVelocity);
19221 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19222 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19223 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19224
19225 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19226 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19227 biasX, biasY, biasZ);
19228
19229
19230 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19231 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19232 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19233 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19234 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19235 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19236 final Acceleration bx2 = new Acceleration(0.0,
19237 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19238 calibrator.getBiasXAsAcceleration(bx2);
19239 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19240 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19241 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19242 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19243 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19244 final Acceleration by2 = new Acceleration(0.0,
19245 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19246 calibrator.getBiasYAsAcceleration(by2);
19247 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19248 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19249 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19250 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19251 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19252 final Acceleration bz2 = new Acceleration(0.0,
19253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19254 calibrator.getBiasZAsAcceleration(bz2);
19255 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19256 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19257 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19258 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19259 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19260 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19261 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19262 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19263 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19264 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19265 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19266 final double[] bias1 = calibrator.getBias();
19267 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19268 final double[] bias2 = new double[3];
19269 calibrator.getBias(bias2);
19270 assertArrayEquals(bias1, bias2, 0.0);
19271 final Matrix b1 = calibrator.getBiasAsMatrix();
19272 assertEquals(b1, ba);
19273 final Matrix b2 = new Matrix(3, 1);
19274 calibrator.getBiasAsMatrix(b2);
19275 assertEquals(b1, b2);
19276 final Matrix ma1 = calibrator.getInitialMa();
19277 assertEquals(ma1, new Matrix(3, 3));
19278 final Matrix ma2 = new Matrix(3, 3);
19279 calibrator.getInitialMa(ma2);
19280 assertEquals(ma1, ma2);
19281 assertNull(calibrator.getMeasurements());
19282 assertFalse(calibrator.isCommonAxisUsed());
19283 assertNull(calibrator.getListener());
19284 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19285 assertFalse(calibrator.isReady());
19286 assertFalse(calibrator.isRunning());
19287 assertNull(calibrator.getEstimatedMa());
19288 assertNull(calibrator.getEstimatedSx());
19289 assertNull(calibrator.getEstimatedSy());
19290 assertNull(calibrator.getEstimatedSz());
19291 assertNull(calibrator.getEstimatedMxy());
19292 assertNull(calibrator.getEstimatedMxz());
19293 assertNull(calibrator.getEstimatedMyx());
19294 assertNull(calibrator.getEstimatedMyz());
19295 assertNull(calibrator.getEstimatedMzx());
19296 assertNull(calibrator.getEstimatedMzy());
19297 assertNull(calibrator.getEstimatedCovariance());
19298 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19299 assertNotNull(calibrator.getGroundTruthGravityNorm());
19300 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19301 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19302 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19303 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19304 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19305 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19306 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19307
19308
19309 final Acceleration invalidGravityNorm = new Acceleration(
19310 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19311
19312 calibrator = null;
19313 try {
19314 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19315 invalidGravityNorm, biasX, biasY, biasZ);
19316 fail("IllegalArgumentException expected but not thrown");
19317 } catch (final IllegalArgumentException ignore) {
19318 }
19319 assertNull(calibrator);
19320 }
19321
19322 @Test
19323 public void testConstructor166() throws WrongSizeException {
19324 final Matrix ba = generateBa();
19325 final double biasX = ba.getElementAtIndex(0);
19326 final double biasY = ba.getElementAtIndex(1);
19327 final double biasZ = ba.getElementAtIndex(2);
19328
19329 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19330 final double latitude = Math.toRadians(
19331 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19332 final double longitude = Math.toRadians(
19333 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19334 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19335 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19336 final NEDVelocity nedVelocity = new NEDVelocity();
19337 final ECEFPosition ecefPosition = new ECEFPosition();
19338 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19339 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19340 ecefPosition, ecefVelocity);
19341 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19342 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19343 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19344
19345 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19346 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19347 biasX, biasY, biasZ, this);
19348
19349
19350 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19351 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19352 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19353 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19354 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19355 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19356 final Acceleration bx2 = new Acceleration(0.0,
19357 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19358 calibrator.getBiasXAsAcceleration(bx2);
19359 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19360 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19361 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19362 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19363 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19364 final Acceleration by2 = new Acceleration(0.0,
19365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19366 calibrator.getBiasYAsAcceleration(by2);
19367 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19368 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19369 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19370 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19371 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19372 final Acceleration bz2 = new Acceleration(0.0,
19373 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19374 calibrator.getBiasZAsAcceleration(bz2);
19375 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19376 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19377 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19378 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19379 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19380 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19381 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19382 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19383 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19384 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19385 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19386 final double[] bias1 = calibrator.getBias();
19387 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19388 final double[] bias2 = new double[3];
19389 calibrator.getBias(bias2);
19390 assertArrayEquals(bias1, bias2, 0.0);
19391 final Matrix b1 = calibrator.getBiasAsMatrix();
19392 assertEquals(b1, ba);
19393 final Matrix b2 = new Matrix(3, 1);
19394 calibrator.getBiasAsMatrix(b2);
19395 assertEquals(b1, b2);
19396 final Matrix ma1 = calibrator.getInitialMa();
19397 assertEquals(ma1, new Matrix(3, 3));
19398 final Matrix ma2 = new Matrix(3, 3);
19399 calibrator.getInitialMa(ma2);
19400 assertEquals(ma1, ma2);
19401 assertNull(calibrator.getMeasurements());
19402 assertFalse(calibrator.isCommonAxisUsed());
19403 assertSame(calibrator.getListener(), this);
19404 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19405 assertFalse(calibrator.isReady());
19406 assertFalse(calibrator.isRunning());
19407 assertNull(calibrator.getEstimatedMa());
19408 assertNull(calibrator.getEstimatedSx());
19409 assertNull(calibrator.getEstimatedSy());
19410 assertNull(calibrator.getEstimatedSz());
19411 assertNull(calibrator.getEstimatedMxy());
19412 assertNull(calibrator.getEstimatedMxz());
19413 assertNull(calibrator.getEstimatedMyx());
19414 assertNull(calibrator.getEstimatedMyz());
19415 assertNull(calibrator.getEstimatedMzx());
19416 assertNull(calibrator.getEstimatedMzy());
19417 assertNull(calibrator.getEstimatedCovariance());
19418 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19419 assertNotNull(calibrator.getGroundTruthGravityNorm());
19420 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19421 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19422 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19423 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19424 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19425 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19426 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19427
19428
19429 final Acceleration invalidGravityNorm = new Acceleration(
19430 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19431
19432 calibrator = null;
19433 try {
19434 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19435 invalidGravityNorm, biasX, biasY, biasZ,
19436 this);
19437 fail("IllegalArgumentException expected but not thrown");
19438 } catch (final IllegalArgumentException ignore) {
19439 }
19440 assertNull(calibrator);
19441 }
19442
19443 @Test
19444 public void testConstructor167() throws WrongSizeException {
19445 final Collection<StandardDeviationBodyKinematics> measurements =
19446 Collections.emptyList();
19447
19448 final Matrix ba = generateBa();
19449 final double biasX = ba.getElementAtIndex(0);
19450 final double biasY = ba.getElementAtIndex(1);
19451 final double biasZ = ba.getElementAtIndex(2);
19452
19453 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19454 final double latitude = Math.toRadians(
19455 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19456 final double longitude = Math.toRadians(
19457 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19458 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19459 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19460 final NEDVelocity nedVelocity = new NEDVelocity();
19461 final ECEFPosition ecefPosition = new ECEFPosition();
19462 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19463 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19464 ecefPosition, ecefVelocity);
19465 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19466 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19467 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19468
19469 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19470 new KnownBiasAndGravityNormAccelerometerCalibrator(
19471 gravityNorm, measurements,
19472 biasX, biasY, biasZ);
19473
19474
19475 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19476 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19477 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19478 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19479 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19480 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19481 final Acceleration bx2 = new Acceleration(0.0,
19482 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19483 calibrator.getBiasXAsAcceleration(bx2);
19484 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19485 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19486 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19487 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19488 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19489 final Acceleration by2 = new Acceleration(0.0,
19490 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19491 calibrator.getBiasYAsAcceleration(by2);
19492 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19493 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19494 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19495 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19496 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19497 final Acceleration bz2 = new Acceleration(0.0,
19498 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19499 calibrator.getBiasZAsAcceleration(bz2);
19500 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19501 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19502 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19503 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19504 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19505 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19506 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19507 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19508 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19509 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19510 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19511 final double[] bias1 = calibrator.getBias();
19512 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19513 final double[] bias2 = new double[3];
19514 calibrator.getBias(bias2);
19515 assertArrayEquals(bias1, bias2, 0.0);
19516 final Matrix b1 = calibrator.getBiasAsMatrix();
19517 assertEquals(b1, ba);
19518 final Matrix b2 = new Matrix(3, 1);
19519 calibrator.getBiasAsMatrix(b2);
19520 assertEquals(b1, b2);
19521 final Matrix ma1 = calibrator.getInitialMa();
19522 assertEquals(ma1, new Matrix(3, 3));
19523 final Matrix ma2 = new Matrix(3, 3);
19524 calibrator.getInitialMa(ma2);
19525 assertEquals(ma1, ma2);
19526 assertSame(calibrator.getMeasurements(), measurements);
19527 assertFalse(calibrator.isCommonAxisUsed());
19528 assertNull(calibrator.getListener());
19529 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19530 assertFalse(calibrator.isReady());
19531 assertFalse(calibrator.isRunning());
19532 assertNull(calibrator.getEstimatedMa());
19533 assertNull(calibrator.getEstimatedSx());
19534 assertNull(calibrator.getEstimatedSy());
19535 assertNull(calibrator.getEstimatedSz());
19536 assertNull(calibrator.getEstimatedMxy());
19537 assertNull(calibrator.getEstimatedMxz());
19538 assertNull(calibrator.getEstimatedMyx());
19539 assertNull(calibrator.getEstimatedMyz());
19540 assertNull(calibrator.getEstimatedMzx());
19541 assertNull(calibrator.getEstimatedMzy());
19542 assertNull(calibrator.getEstimatedCovariance());
19543 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19544 assertNotNull(calibrator.getGroundTruthGravityNorm());
19545 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19546 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19547 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19548 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19549 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19550 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19551 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19552
19553
19554 final Acceleration invalidGravityNorm = new Acceleration(
19555 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19556
19557 calibrator = null;
19558 try {
19559 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19560 invalidGravityNorm, measurements,
19561 biasX, biasY, biasZ);
19562 fail("IllegalArgumentException expected but not thrown");
19563 } catch (final IllegalArgumentException ignore) {
19564 }
19565 assertNull(calibrator);
19566 }
19567
19568 @Test
19569 public void testConstructor168() throws WrongSizeException {
19570 final Collection<StandardDeviationBodyKinematics> measurements =
19571 Collections.emptyList();
19572
19573 final Matrix ba = generateBa();
19574 final double biasX = ba.getElementAtIndex(0);
19575 final double biasY = ba.getElementAtIndex(1);
19576 final double biasZ = ba.getElementAtIndex(2);
19577
19578 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19579 final double latitude = Math.toRadians(
19580 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19581 final double longitude = Math.toRadians(
19582 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19583 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19584 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19585 final NEDVelocity nedVelocity = new NEDVelocity();
19586 final ECEFPosition ecefPosition = new ECEFPosition();
19587 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19588 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19589 ecefPosition, ecefVelocity);
19590 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19591 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19592 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19593
19594 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19595 new KnownBiasAndGravityNormAccelerometerCalibrator(
19596 gravityNorm, measurements,
19597 biasX, biasY, biasZ, this);
19598
19599
19600 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19601 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19602 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19603 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19604 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19605 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19606 final Acceleration bx2 = new Acceleration(0.0,
19607 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19608 calibrator.getBiasXAsAcceleration(bx2);
19609 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19610 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19611 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19612 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19613 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19614 final Acceleration by2 = new Acceleration(0.0,
19615 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19616 calibrator.getBiasYAsAcceleration(by2);
19617 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19618 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19619 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19620 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19621 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19622 final Acceleration bz2 = new Acceleration(0.0,
19623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19624 calibrator.getBiasZAsAcceleration(bz2);
19625 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19626 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19627 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19628 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19629 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19630 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19631 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19632 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19633 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19634 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19635 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19636 final double[] bias1 = calibrator.getBias();
19637 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19638 final double[] bias2 = new double[3];
19639 calibrator.getBias(bias2);
19640 assertArrayEquals(bias1, bias2, 0.0);
19641 final Matrix b1 = calibrator.getBiasAsMatrix();
19642 assertEquals(b1, ba);
19643 final Matrix b2 = new Matrix(3, 1);
19644 calibrator.getBiasAsMatrix(b2);
19645 assertEquals(b1, b2);
19646 final Matrix ma1 = calibrator.getInitialMa();
19647 assertEquals(ma1, new Matrix(3, 3));
19648 final Matrix ma2 = new Matrix(3, 3);
19649 calibrator.getInitialMa(ma2);
19650 assertEquals(ma1, ma2);
19651 assertSame(calibrator.getMeasurements(), measurements);
19652 assertFalse(calibrator.isCommonAxisUsed());
19653 assertSame(calibrator.getListener(), this);
19654 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19655 assertFalse(calibrator.isReady());
19656 assertFalse(calibrator.isRunning());
19657 assertNull(calibrator.getEstimatedMa());
19658 assertNull(calibrator.getEstimatedSx());
19659 assertNull(calibrator.getEstimatedSy());
19660 assertNull(calibrator.getEstimatedSz());
19661 assertNull(calibrator.getEstimatedMxy());
19662 assertNull(calibrator.getEstimatedMxz());
19663 assertNull(calibrator.getEstimatedMyx());
19664 assertNull(calibrator.getEstimatedMyz());
19665 assertNull(calibrator.getEstimatedMzx());
19666 assertNull(calibrator.getEstimatedMzy());
19667 assertNull(calibrator.getEstimatedCovariance());
19668 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19669 assertNotNull(calibrator.getGroundTruthGravityNorm());
19670 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19671 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19672 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19673 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19674 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19675 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19676 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19677
19678
19679 final Acceleration invalidGravityNorm = new Acceleration(
19680 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19681
19682 calibrator = null;
19683 try {
19684 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19685 invalidGravityNorm, measurements,
19686 biasX, biasY, biasZ, this);
19687 fail("IllegalArgumentException expected but not thrown");
19688 } catch (final IllegalArgumentException ignore) {
19689 }
19690 assertNull(calibrator);
19691 }
19692
19693 @Test
19694 public void testConstructor169() throws WrongSizeException {
19695 final Matrix ba = generateBa();
19696 final double biasX = ba.getElementAtIndex(0);
19697 final double biasY = ba.getElementAtIndex(1);
19698 final double biasZ = ba.getElementAtIndex(2);
19699
19700 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19701 final double latitude = Math.toRadians(
19702 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19703 final double longitude = Math.toRadians(
19704 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19705 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19706 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19707 final NEDVelocity nedVelocity = new NEDVelocity();
19708 final ECEFPosition ecefPosition = new ECEFPosition();
19709 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19710 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19711 ecefPosition, ecefVelocity);
19712 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19713 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19714 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19715
19716 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19717 new KnownBiasAndGravityNormAccelerometerCalibrator(
19718 gravityNorm, true, biasX, biasY, biasZ);
19719
19720
19721 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19722 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19723 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19724 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19725 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19726 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19727 final Acceleration bx2 = new Acceleration(0.0,
19728 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19729 calibrator.getBiasXAsAcceleration(bx2);
19730 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19731 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19732 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19733 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19734 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19735 final Acceleration by2 = new Acceleration(0.0,
19736 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19737 calibrator.getBiasYAsAcceleration(by2);
19738 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19739 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19740 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19741 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19742 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19743 final Acceleration bz2 = new Acceleration(0.0,
19744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19745 calibrator.getBiasZAsAcceleration(bz2);
19746 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19747 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19748 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19749 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19750 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19751 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19752 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19753 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19754 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19755 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19756 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19757 final double[] bias1 = calibrator.getBias();
19758 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19759 final double[] bias2 = new double[3];
19760 calibrator.getBias(bias2);
19761 assertArrayEquals(bias1, bias2, 0.0);
19762 final Matrix b1 = calibrator.getBiasAsMatrix();
19763 assertEquals(b1, ba);
19764 final Matrix b2 = new Matrix(3, 1);
19765 calibrator.getBiasAsMatrix(b2);
19766 assertEquals(b1, b2);
19767 final Matrix ma1 = calibrator.getInitialMa();
19768 assertEquals(ma1, new Matrix(3, 3));
19769 final Matrix ma2 = new Matrix(3, 3);
19770 calibrator.getInitialMa(ma2);
19771 assertEquals(ma1, ma2);
19772 assertNull(calibrator.getMeasurements());
19773 assertTrue(calibrator.isCommonAxisUsed());
19774 assertNull(calibrator.getListener());
19775 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19776 assertFalse(calibrator.isReady());
19777 assertFalse(calibrator.isRunning());
19778 assertNull(calibrator.getEstimatedMa());
19779 assertNull(calibrator.getEstimatedSx());
19780 assertNull(calibrator.getEstimatedSy());
19781 assertNull(calibrator.getEstimatedSz());
19782 assertNull(calibrator.getEstimatedMxy());
19783 assertNull(calibrator.getEstimatedMxz());
19784 assertNull(calibrator.getEstimatedMyx());
19785 assertNull(calibrator.getEstimatedMyz());
19786 assertNull(calibrator.getEstimatedMzx());
19787 assertNull(calibrator.getEstimatedMzy());
19788 assertNull(calibrator.getEstimatedCovariance());
19789 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19790 assertNotNull(calibrator.getGroundTruthGravityNorm());
19791 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19792 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19793 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19794 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19795 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19796 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19797 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19798
19799
19800 final Acceleration invalidGravityNorm = new Acceleration(
19801 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19802
19803 calibrator = null;
19804 try {
19805 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19806 invalidGravityNorm, true, biasX, biasY, biasZ);
19807 fail("IllegalArgumentException expected but not thrown");
19808 } catch (final IllegalArgumentException ignore) {
19809 }
19810 assertNull(calibrator);
19811 }
19812
19813 @Test
19814 public void testConstructor170() throws WrongSizeException {
19815 final Matrix ba = generateBa();
19816 final double biasX = ba.getElementAtIndex(0);
19817 final double biasY = ba.getElementAtIndex(1);
19818 final double biasZ = ba.getElementAtIndex(2);
19819
19820 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19821 final double latitude = Math.toRadians(
19822 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19823 final double longitude = Math.toRadians(
19824 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19825 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19826 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19827 final NEDVelocity nedVelocity = new NEDVelocity();
19828 final ECEFPosition ecefPosition = new ECEFPosition();
19829 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19830 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19831 ecefPosition, ecefVelocity);
19832 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19833 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19834 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19835
19836 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19837 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19838 true, biasX, biasY, biasZ, this);
19839
19840
19841 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19842 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19843 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19844 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19845 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19846 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19847 final Acceleration bx2 = new Acceleration(0.0,
19848 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19849 calibrator.getBiasXAsAcceleration(bx2);
19850 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19851 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19852 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19853 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19854 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19855 final Acceleration by2 = new Acceleration(0.0,
19856 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19857 calibrator.getBiasYAsAcceleration(by2);
19858 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19859 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19860 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19861 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19862 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19863 final Acceleration bz2 = new Acceleration(0.0,
19864 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19865 calibrator.getBiasZAsAcceleration(bz2);
19866 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19867 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19868 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19869 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19870 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19871 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19872 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19873 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19874 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19875 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19876 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19877 final double[] bias1 = calibrator.getBias();
19878 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19879 final double[] bias2 = new double[3];
19880 calibrator.getBias(bias2);
19881 assertArrayEquals(bias1, bias2, 0.0);
19882 final Matrix b1 = calibrator.getBiasAsMatrix();
19883 assertEquals(b1, ba);
19884 final Matrix b2 = new Matrix(3, 1);
19885 calibrator.getBiasAsMatrix(b2);
19886 assertEquals(b1, b2);
19887 final Matrix ma1 = calibrator.getInitialMa();
19888 assertEquals(ma1, new Matrix(3, 3));
19889 final Matrix ma2 = new Matrix(3, 3);
19890 calibrator.getInitialMa(ma2);
19891 assertEquals(ma1, ma2);
19892 assertNull(calibrator.getMeasurements());
19893 assertTrue(calibrator.isCommonAxisUsed());
19894 assertSame(calibrator.getListener(), this);
19895 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19896 assertFalse(calibrator.isReady());
19897 assertFalse(calibrator.isRunning());
19898 assertNull(calibrator.getEstimatedMa());
19899 assertNull(calibrator.getEstimatedSx());
19900 assertNull(calibrator.getEstimatedSy());
19901 assertNull(calibrator.getEstimatedSz());
19902 assertNull(calibrator.getEstimatedMxy());
19903 assertNull(calibrator.getEstimatedMxz());
19904 assertNull(calibrator.getEstimatedMyx());
19905 assertNull(calibrator.getEstimatedMyz());
19906 assertNull(calibrator.getEstimatedMzx());
19907 assertNull(calibrator.getEstimatedMzy());
19908 assertNull(calibrator.getEstimatedCovariance());
19909 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19910 assertNotNull(calibrator.getGroundTruthGravityNorm());
19911 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19912 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19913 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19914 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19915 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19916 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19917 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19918
19919
19920 final Acceleration invalidGravityNorm = new Acceleration(
19921 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19922
19923 calibrator = null;
19924 try {
19925 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19926 invalidGravityNorm, true,
19927 biasX, biasY, biasZ, this);
19928 fail("IllegalArgumentException expected but not thrown");
19929 } catch (final IllegalArgumentException ignore) {
19930 }
19931 assertNull(calibrator);
19932 }
19933
19934 @Test
19935 public void testConstructor171() throws WrongSizeException {
19936 final Collection<StandardDeviationBodyKinematics> measurements =
19937 Collections.emptyList();
19938
19939 final Matrix ba = generateBa();
19940 final double biasX = ba.getElementAtIndex(0);
19941 final double biasY = ba.getElementAtIndex(1);
19942 final double biasZ = ba.getElementAtIndex(2);
19943
19944 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19945 final double latitude = Math.toRadians(
19946 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19947 final double longitude = Math.toRadians(
19948 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19949 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19950 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19951 final NEDVelocity nedVelocity = new NEDVelocity();
19952 final ECEFPosition ecefPosition = new ECEFPosition();
19953 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19954 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19955 ecefPosition, ecefVelocity);
19956 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19957 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19958 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19959
19960 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19961 new KnownBiasAndGravityNormAccelerometerCalibrator(
19962 gravityNorm, measurements,
19963 true, biasX, biasY, biasZ);
19964
19965
19966 assertEquals(calibrator.getBiasX(), biasX, 0.0);
19967 assertEquals(calibrator.getBiasY(), biasY, 0.0);
19968 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19969 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19970 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19971 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19972 final Acceleration bx2 = new Acceleration(0.0,
19973 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19974 calibrator.getBiasXAsAcceleration(bx2);
19975 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19976 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19977 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19978 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19979 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19980 final Acceleration by2 = new Acceleration(0.0,
19981 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19982 calibrator.getBiasYAsAcceleration(by2);
19983 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19984 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19985 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19986 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19987 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19988 final Acceleration bz2 = new Acceleration(0.0,
19989 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19990 calibrator.getBiasZAsAcceleration(bz2);
19991 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19992 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19993 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19994 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19995 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19996 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19997 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19998 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19999 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20000 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20001 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20002 final double[] bias1 = calibrator.getBias();
20003 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20004 final double[] bias2 = new double[3];
20005 calibrator.getBias(bias2);
20006 assertArrayEquals(bias1, bias2, 0.0);
20007 final Matrix b1 = calibrator.getBiasAsMatrix();
20008 assertEquals(b1, ba);
20009 final Matrix b2 = new Matrix(3, 1);
20010 calibrator.getBiasAsMatrix(b2);
20011 assertEquals(b1, b2);
20012 final Matrix ma1 = calibrator.getInitialMa();
20013 assertEquals(ma1, new Matrix(3, 3));
20014 final Matrix ma2 = new Matrix(3, 3);
20015 calibrator.getInitialMa(ma2);
20016 assertEquals(ma1, ma2);
20017 assertSame(calibrator.getMeasurements(), measurements);
20018 assertTrue(calibrator.isCommonAxisUsed());
20019 assertNull(calibrator.getListener());
20020 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20021 assertFalse(calibrator.isReady());
20022 assertFalse(calibrator.isRunning());
20023 assertNull(calibrator.getEstimatedMa());
20024 assertNull(calibrator.getEstimatedSx());
20025 assertNull(calibrator.getEstimatedSy());
20026 assertNull(calibrator.getEstimatedSz());
20027 assertNull(calibrator.getEstimatedMxy());
20028 assertNull(calibrator.getEstimatedMxz());
20029 assertNull(calibrator.getEstimatedMyx());
20030 assertNull(calibrator.getEstimatedMyz());
20031 assertNull(calibrator.getEstimatedMzx());
20032 assertNull(calibrator.getEstimatedMzy());
20033 assertNull(calibrator.getEstimatedCovariance());
20034 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20035 assertNotNull(calibrator.getGroundTruthGravityNorm());
20036 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20037 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20038 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20039 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20040 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20041 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20042 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20043
20044
20045 final Acceleration invalidGravityNorm = new Acceleration(
20046 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20047
20048 calibrator = null;
20049 try {
20050 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20051 invalidGravityNorm, measurements,
20052 true, biasX, biasY, biasZ);
20053 fail("IllegalArgumentException expected but not thrown");
20054 } catch (final IllegalArgumentException ignore) {
20055 }
20056 assertNull(calibrator);
20057 }
20058
20059 @Test
20060 public void testConstructor172() throws WrongSizeException {
20061 final Collection<StandardDeviationBodyKinematics> measurements =
20062 Collections.emptyList();
20063
20064 final Matrix ba = generateBa();
20065 final double biasX = ba.getElementAtIndex(0);
20066 final double biasY = ba.getElementAtIndex(1);
20067 final double biasZ = ba.getElementAtIndex(2);
20068
20069 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20070 final double latitude = Math.toRadians(
20071 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20072 final double longitude = Math.toRadians(
20073 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20074 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20075 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20076 final NEDVelocity nedVelocity = new NEDVelocity();
20077 final ECEFPosition ecefPosition = new ECEFPosition();
20078 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20079 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20080 ecefPosition, ecefVelocity);
20081 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20082 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20083 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20084
20085 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20086 new KnownBiasAndGravityNormAccelerometerCalibrator(
20087 gravityNorm, measurements,
20088 true, biasX, biasY, biasZ, this);
20089
20090
20091 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20092 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20093 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20094 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20095 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20096 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20097 final Acceleration bx2 = new Acceleration(0.0,
20098 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20099 calibrator.getBiasXAsAcceleration(bx2);
20100 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20101 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20102 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20103 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20104 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20105 final Acceleration by2 = new Acceleration(0.0,
20106 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20107 calibrator.getBiasYAsAcceleration(by2);
20108 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20109 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20110 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20111 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20112 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20113 final Acceleration bz2 = new Acceleration(0.0,
20114 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20115 calibrator.getBiasZAsAcceleration(bz2);
20116 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20117 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20118 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20119 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20120 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20121 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20122 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20123 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20124 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20125 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20126 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20127 final double[] bias1 = calibrator.getBias();
20128 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20129 final double[] bias2 = new double[3];
20130 calibrator.getBias(bias2);
20131 assertArrayEquals(bias1, bias2, 0.0);
20132 final Matrix b1 = calibrator.getBiasAsMatrix();
20133 assertEquals(b1, ba);
20134 final Matrix b2 = new Matrix(3, 1);
20135 calibrator.getBiasAsMatrix(b2);
20136 assertEquals(b1, b2);
20137 final Matrix ma1 = calibrator.getInitialMa();
20138 assertEquals(ma1, new Matrix(3, 3));
20139 final Matrix ma2 = new Matrix(3, 3);
20140 calibrator.getInitialMa(ma2);
20141 assertEquals(ma1, ma2);
20142 assertSame(calibrator.getMeasurements(), measurements);
20143 assertTrue(calibrator.isCommonAxisUsed());
20144 assertSame(calibrator.getListener(), this);
20145 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20146 assertFalse(calibrator.isReady());
20147 assertFalse(calibrator.isRunning());
20148 assertNull(calibrator.getEstimatedMa());
20149 assertNull(calibrator.getEstimatedSx());
20150 assertNull(calibrator.getEstimatedSy());
20151 assertNull(calibrator.getEstimatedSz());
20152 assertNull(calibrator.getEstimatedMxy());
20153 assertNull(calibrator.getEstimatedMxz());
20154 assertNull(calibrator.getEstimatedMyx());
20155 assertNull(calibrator.getEstimatedMyz());
20156 assertNull(calibrator.getEstimatedMzx());
20157 assertNull(calibrator.getEstimatedMzy());
20158 assertNull(calibrator.getEstimatedCovariance());
20159 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20160 assertNotNull(calibrator.getGroundTruthGravityNorm());
20161 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20162 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20163 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20164 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20165 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20166 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20167 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20168
20169
20170 final Acceleration invalidGravityNorm = new Acceleration(
20171 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20172
20173 calibrator = null;
20174 try {
20175 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20176 invalidGravityNorm, measurements,
20177 true, biasX, biasY, biasZ, this);
20178 fail("IllegalArgumentException expected but not thrown");
20179 } catch (final IllegalArgumentException ignore) {
20180 }
20181 assertNull(calibrator);
20182 }
20183
20184 @Test
20185 public void testConstructor173() throws WrongSizeException {
20186 final Matrix ba = generateBa();
20187 final double biasX = ba.getElementAtIndex(0);
20188 final double biasY = ba.getElementAtIndex(1);
20189 final double biasZ = ba.getElementAtIndex(2);
20190
20191 final Acceleration bx = new Acceleration(biasX,
20192 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20193 final Acceleration by = new Acceleration(biasY,
20194 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20195 final Acceleration bz = new Acceleration(biasZ,
20196 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20197
20198 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20199 final double latitude = Math.toRadians(
20200 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20201 final double longitude = Math.toRadians(
20202 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20203 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20204 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20205 final NEDVelocity nedVelocity = new NEDVelocity();
20206 final ECEFPosition ecefPosition = new ECEFPosition();
20207 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20208 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20209 ecefPosition, ecefVelocity);
20210 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20211 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20212 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20213
20214 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20215 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20216 bx, by, bz);
20217
20218
20219 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20220 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20221 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20222 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20223 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20224 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20225 final Acceleration bx2 = new Acceleration(0.0,
20226 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20227 calibrator.getBiasXAsAcceleration(bx2);
20228 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20229 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20230 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20231 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20232 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20233 final Acceleration by2 = new Acceleration(0.0,
20234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20235 calibrator.getBiasYAsAcceleration(by2);
20236 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20237 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20238 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20239 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20240 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20241 final Acceleration bz2 = new Acceleration(0.0,
20242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20243 calibrator.getBiasZAsAcceleration(bz2);
20244 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20245 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20246 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20247 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20248 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20249 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20250 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20251 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20252 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20253 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20254 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20255 final double[] bias1 = calibrator.getBias();
20256 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20257 final double[] bias2 = new double[3];
20258 calibrator.getBias(bias2);
20259 assertArrayEquals(bias1, bias2, 0.0);
20260 final Matrix b1 = calibrator.getBiasAsMatrix();
20261 assertEquals(b1, ba);
20262 final Matrix b2 = new Matrix(3, 1);
20263 calibrator.getBiasAsMatrix(b2);
20264 assertEquals(b1, b2);
20265 final Matrix ma1 = calibrator.getInitialMa();
20266 assertEquals(ma1, new Matrix(3, 3));
20267 final Matrix ma2 = new Matrix(3, 3);
20268 calibrator.getInitialMa(ma2);
20269 assertEquals(ma1, ma2);
20270 assertNull(calibrator.getMeasurements());
20271 assertFalse(calibrator.isCommonAxisUsed());
20272 assertNull(calibrator.getListener());
20273 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20274 assertFalse(calibrator.isReady());
20275 assertFalse(calibrator.isRunning());
20276 assertNull(calibrator.getEstimatedMa());
20277 assertNull(calibrator.getEstimatedSx());
20278 assertNull(calibrator.getEstimatedSy());
20279 assertNull(calibrator.getEstimatedSz());
20280 assertNull(calibrator.getEstimatedMxy());
20281 assertNull(calibrator.getEstimatedMxz());
20282 assertNull(calibrator.getEstimatedMyx());
20283 assertNull(calibrator.getEstimatedMyz());
20284 assertNull(calibrator.getEstimatedMzx());
20285 assertNull(calibrator.getEstimatedMzy());
20286 assertNull(calibrator.getEstimatedCovariance());
20287 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20288 assertNotNull(calibrator.getGroundTruthGravityNorm());
20289 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20290 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20291 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20292 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20293 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20294 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20295 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20296
20297
20298 final Acceleration invalidGravityNorm = new Acceleration(
20299 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20300
20301 calibrator = null;
20302 try {
20303 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20304 invalidGravityNorm, bx, by, bz);
20305 fail("IllegalArgumentException expected but not thrown");
20306 } catch (final IllegalArgumentException ignore) {
20307 }
20308 assertNull(calibrator);
20309 }
20310
20311 @Test
20312 public void testConstructor174() throws WrongSizeException {
20313 final Matrix ba = generateBa();
20314 final double biasX = ba.getElementAtIndex(0);
20315 final double biasY = ba.getElementAtIndex(1);
20316 final double biasZ = ba.getElementAtIndex(2);
20317
20318 final Acceleration bx = new Acceleration(biasX,
20319 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20320 final Acceleration by = new Acceleration(biasY,
20321 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20322 final Acceleration bz = new Acceleration(biasZ,
20323 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20324
20325 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20326 final double latitude = Math.toRadians(
20327 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20328 final double longitude = Math.toRadians(
20329 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20330 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20331 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20332 final NEDVelocity nedVelocity = new NEDVelocity();
20333 final ECEFPosition ecefPosition = new ECEFPosition();
20334 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20335 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20336 ecefPosition, ecefVelocity);
20337 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20338 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20339 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20340
20341 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20342 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20343 bx, by, bz, this);
20344
20345
20346 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20347 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20348 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20349 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20350 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20351 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20352 final Acceleration bx2 = new Acceleration(0.0,
20353 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20354 calibrator.getBiasXAsAcceleration(bx2);
20355 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20356 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20357 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20358 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20359 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20360 final Acceleration by2 = new Acceleration(0.0,
20361 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20362 calibrator.getBiasYAsAcceleration(by2);
20363 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20364 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20365 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20366 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20367 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20368 final Acceleration bz2 = new Acceleration(0.0,
20369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20370 calibrator.getBiasZAsAcceleration(bz2);
20371 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20372 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20373 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20374 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20375 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20376 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20377 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20378 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20379 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20380 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20381 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20382 final double[] bias1 = calibrator.getBias();
20383 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20384 final double[] bias2 = new double[3];
20385 calibrator.getBias(bias2);
20386 assertArrayEquals(bias1, bias2, 0.0);
20387 final Matrix b1 = calibrator.getBiasAsMatrix();
20388 assertEquals(b1, ba);
20389 final Matrix b2 = new Matrix(3, 1);
20390 calibrator.getBiasAsMatrix(b2);
20391 assertEquals(b1, b2);
20392 final Matrix ma1 = calibrator.getInitialMa();
20393 assertEquals(ma1, new Matrix(3, 3));
20394 final Matrix ma2 = new Matrix(3, 3);
20395 calibrator.getInitialMa(ma2);
20396 assertEquals(ma1, ma2);
20397 assertNull(calibrator.getMeasurements());
20398 assertFalse(calibrator.isCommonAxisUsed());
20399 assertSame(calibrator.getListener(), this);
20400 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20401 assertFalse(calibrator.isReady());
20402 assertFalse(calibrator.isRunning());
20403 assertNull(calibrator.getEstimatedMa());
20404 assertNull(calibrator.getEstimatedSx());
20405 assertNull(calibrator.getEstimatedSy());
20406 assertNull(calibrator.getEstimatedSz());
20407 assertNull(calibrator.getEstimatedMxy());
20408 assertNull(calibrator.getEstimatedMxz());
20409 assertNull(calibrator.getEstimatedMyx());
20410 assertNull(calibrator.getEstimatedMyz());
20411 assertNull(calibrator.getEstimatedMzx());
20412 assertNull(calibrator.getEstimatedMzy());
20413 assertNull(calibrator.getEstimatedCovariance());
20414 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20415 assertNotNull(calibrator.getGroundTruthGravityNorm());
20416 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20417 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20418 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20419 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20420 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20421 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20422 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20423
20424
20425 final Acceleration invalidGravityNorm = new Acceleration(
20426 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20427
20428 calibrator = null;
20429 try {
20430 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20431 invalidGravityNorm, bx, by, bz, this);
20432 fail("IllegalArgumentException expected but not thrown");
20433 } catch (final IllegalArgumentException ignore) {
20434 }
20435 assertNull(calibrator);
20436 }
20437
20438 @Test
20439 public void testConstructor175() throws WrongSizeException {
20440 final Collection<StandardDeviationBodyKinematics> measurements =
20441 Collections.emptyList();
20442
20443 final Matrix ba = generateBa();
20444 final double biasX = ba.getElementAtIndex(0);
20445 final double biasY = ba.getElementAtIndex(1);
20446 final double biasZ = ba.getElementAtIndex(2);
20447
20448 final Acceleration bx = new Acceleration(biasX,
20449 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20450 final Acceleration by = new Acceleration(biasY,
20451 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20452 final Acceleration bz = new Acceleration(biasZ,
20453 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20454
20455 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20456 final double latitude = Math.toRadians(
20457 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20458 final double longitude = Math.toRadians(
20459 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20460 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20461 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20462 final NEDVelocity nedVelocity = new NEDVelocity();
20463 final ECEFPosition ecefPosition = new ECEFPosition();
20464 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20465 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20466 ecefPosition, ecefVelocity);
20467 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20468 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20469 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20470
20471 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20472 new KnownBiasAndGravityNormAccelerometerCalibrator(
20473 gravityNorm, measurements, bx, by, bz);
20474
20475
20476 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20477 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20478 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20479 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20480 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20481 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20482 final Acceleration bx2 = new Acceleration(0.0,
20483 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20484 calibrator.getBiasXAsAcceleration(bx2);
20485 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20486 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20487 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20488 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20489 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20490 final Acceleration by2 = new Acceleration(0.0,
20491 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20492 calibrator.getBiasYAsAcceleration(by2);
20493 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20494 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20495 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20496 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20497 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20498 final Acceleration bz2 = new Acceleration(0.0,
20499 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20500 calibrator.getBiasZAsAcceleration(bz2);
20501 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20502 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20503 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20504 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20505 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20506 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20507 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20508 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20509 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20510 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20511 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20512 final double[] bias1 = calibrator.getBias();
20513 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20514 final double[] bias2 = new double[3];
20515 calibrator.getBias(bias2);
20516 assertArrayEquals(bias1, bias2, 0.0);
20517 final Matrix b1 = calibrator.getBiasAsMatrix();
20518 assertEquals(b1, ba);
20519 final Matrix b2 = new Matrix(3, 1);
20520 calibrator.getBiasAsMatrix(b2);
20521 assertEquals(b1, b2);
20522 final Matrix ma1 = calibrator.getInitialMa();
20523 assertEquals(ma1, new Matrix(3, 3));
20524 final Matrix ma2 = new Matrix(3, 3);
20525 calibrator.getInitialMa(ma2);
20526 assertEquals(ma1, ma2);
20527 assertSame(calibrator.getMeasurements(), measurements);
20528 assertFalse(calibrator.isCommonAxisUsed());
20529 assertNull(calibrator.getListener());
20530 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20531 assertFalse(calibrator.isReady());
20532 assertFalse(calibrator.isRunning());
20533 assertNull(calibrator.getEstimatedMa());
20534 assertNull(calibrator.getEstimatedSx());
20535 assertNull(calibrator.getEstimatedSy());
20536 assertNull(calibrator.getEstimatedSz());
20537 assertNull(calibrator.getEstimatedMxy());
20538 assertNull(calibrator.getEstimatedMxz());
20539 assertNull(calibrator.getEstimatedMyx());
20540 assertNull(calibrator.getEstimatedMyz());
20541 assertNull(calibrator.getEstimatedMzx());
20542 assertNull(calibrator.getEstimatedMzy());
20543 assertNull(calibrator.getEstimatedCovariance());
20544 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20545 assertNotNull(calibrator.getGroundTruthGravityNorm());
20546 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20547 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20548 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20549 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20550 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20551 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20552 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20553
20554
20555 final Acceleration invalidGravityNorm = new Acceleration(
20556 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20557
20558 calibrator = null;
20559 try {
20560 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20561 invalidGravityNorm, measurements, bx, by, bz);
20562 fail("IllegalArgumentException expected but not thrown");
20563 } catch (final IllegalArgumentException ignore) {
20564 }
20565 assertNull(calibrator);
20566 }
20567
20568 @Test
20569 public void testConstructor176() throws WrongSizeException {
20570 final Collection<StandardDeviationBodyKinematics> measurements =
20571 Collections.emptyList();
20572
20573 final Matrix ba = generateBa();
20574 final double biasX = ba.getElementAtIndex(0);
20575 final double biasY = ba.getElementAtIndex(1);
20576 final double biasZ = ba.getElementAtIndex(2);
20577
20578 final Acceleration bx = new Acceleration(biasX,
20579 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20580 final Acceleration by = new Acceleration(biasY,
20581 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20582 final Acceleration bz = new Acceleration(biasZ,
20583 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20584
20585 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20586 final double latitude = Math.toRadians(
20587 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20588 final double longitude = Math.toRadians(
20589 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20590 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20591 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20592 final NEDVelocity nedVelocity = new NEDVelocity();
20593 final ECEFPosition ecefPosition = new ECEFPosition();
20594 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20595 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20596 ecefPosition, ecefVelocity);
20597 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20598 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20599 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20600
20601 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20602 new KnownBiasAndGravityNormAccelerometerCalibrator(
20603 gravityNorm, measurements,
20604 bx, by, bz, this);
20605
20606
20607 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20608 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20609 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20610 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20611 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20612 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20613 final Acceleration bx2 = new Acceleration(0.0,
20614 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20615 calibrator.getBiasXAsAcceleration(bx2);
20616 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20617 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20618 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20619 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20620 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20621 final Acceleration by2 = new Acceleration(0.0,
20622 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20623 calibrator.getBiasYAsAcceleration(by2);
20624 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20625 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20626 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20627 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20628 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20629 final Acceleration bz2 = new Acceleration(0.0,
20630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20631 calibrator.getBiasZAsAcceleration(bz2);
20632 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20633 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20634 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20635 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20636 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20637 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20638 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20639 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20640 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20641 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20642 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20643 final double[] bias1 = calibrator.getBias();
20644 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20645 final double[] bias2 = new double[3];
20646 calibrator.getBias(bias2);
20647 assertArrayEquals(bias1, bias2, 0.0);
20648 final Matrix b1 = calibrator.getBiasAsMatrix();
20649 assertEquals(b1, ba);
20650 final Matrix b2 = new Matrix(3, 1);
20651 calibrator.getBiasAsMatrix(b2);
20652 assertEquals(b1, b2);
20653 final Matrix ma1 = calibrator.getInitialMa();
20654 assertEquals(ma1, new Matrix(3, 3));
20655 final Matrix ma2 = new Matrix(3, 3);
20656 calibrator.getInitialMa(ma2);
20657 assertEquals(ma1, ma2);
20658 assertSame(calibrator.getMeasurements(), measurements);
20659 assertFalse(calibrator.isCommonAxisUsed());
20660 assertSame(calibrator.getListener(), this);
20661 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20662 assertFalse(calibrator.isReady());
20663 assertFalse(calibrator.isRunning());
20664 assertNull(calibrator.getEstimatedMa());
20665 assertNull(calibrator.getEstimatedSx());
20666 assertNull(calibrator.getEstimatedSy());
20667 assertNull(calibrator.getEstimatedSz());
20668 assertNull(calibrator.getEstimatedMxy());
20669 assertNull(calibrator.getEstimatedMxz());
20670 assertNull(calibrator.getEstimatedMyx());
20671 assertNull(calibrator.getEstimatedMyz());
20672 assertNull(calibrator.getEstimatedMzx());
20673 assertNull(calibrator.getEstimatedMzy());
20674 assertNull(calibrator.getEstimatedCovariance());
20675 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20676 assertNotNull(calibrator.getGroundTruthGravityNorm());
20677 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20678 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20679 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20680 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20681 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20682 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20683 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20684
20685
20686 final Acceleration invalidGravityNorm = new Acceleration(
20687 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20688
20689 calibrator = null;
20690 try {
20691 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20692 invalidGravityNorm, measurements,
20693 bx, by, bz, this);
20694 fail("IllegalArgumentException expected but not thrown");
20695 } catch (final IllegalArgumentException ignore) {
20696 }
20697 assertNull(calibrator);
20698 }
20699
20700 @Test
20701 public void testConstructor177() throws WrongSizeException {
20702 final Matrix ba = generateBa();
20703 final double biasX = ba.getElementAtIndex(0);
20704 final double biasY = ba.getElementAtIndex(1);
20705 final double biasZ = ba.getElementAtIndex(2);
20706
20707 final Acceleration bx = new Acceleration(biasX,
20708 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20709 final Acceleration by = new Acceleration(biasY,
20710 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20711 final Acceleration bz = new Acceleration(biasZ,
20712 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20713
20714 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20715 final double latitude = Math.toRadians(
20716 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20717 final double longitude = Math.toRadians(
20718 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20719 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20720 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20721 final NEDVelocity nedVelocity = new NEDVelocity();
20722 final ECEFPosition ecefPosition = new ECEFPosition();
20723 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20724 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20725 ecefPosition, ecefVelocity);
20726 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20727 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20728 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20729
20730 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20731 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20732 true, bx, by, bz);
20733
20734
20735 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20736 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20737 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20738 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20739 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20740 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20741 final Acceleration bx2 = new Acceleration(0.0,
20742 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20743 calibrator.getBiasXAsAcceleration(bx2);
20744 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20745 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20746 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20747 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20748 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20749 final Acceleration by2 = new Acceleration(0.0,
20750 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20751 calibrator.getBiasYAsAcceleration(by2);
20752 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20753 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20754 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20755 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20756 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20757 final Acceleration bz2 = new Acceleration(0.0,
20758 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20759 calibrator.getBiasZAsAcceleration(bz2);
20760 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20761 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20762 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20763 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20764 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20765 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20766 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20767 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20768 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20769 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20770 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20771 final double[] bias1 = calibrator.getBias();
20772 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20773 final double[] bias2 = new double[3];
20774 calibrator.getBias(bias2);
20775 assertArrayEquals(bias1, bias2, 0.0);
20776 final Matrix b1 = calibrator.getBiasAsMatrix();
20777 assertEquals(b1, ba);
20778 final Matrix b2 = new Matrix(3, 1);
20779 calibrator.getBiasAsMatrix(b2);
20780 assertEquals(b1, b2);
20781 final Matrix ma1 = calibrator.getInitialMa();
20782 assertEquals(ma1, new Matrix(3, 3));
20783 final Matrix ma2 = new Matrix(3, 3);
20784 calibrator.getInitialMa(ma2);
20785 assertEquals(ma1, ma2);
20786 assertNull(calibrator.getMeasurements());
20787 assertTrue(calibrator.isCommonAxisUsed());
20788 assertNull(calibrator.getListener());
20789 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20790 assertFalse(calibrator.isReady());
20791 assertFalse(calibrator.isRunning());
20792 assertNull(calibrator.getEstimatedMa());
20793 assertNull(calibrator.getEstimatedSx());
20794 assertNull(calibrator.getEstimatedSy());
20795 assertNull(calibrator.getEstimatedSz());
20796 assertNull(calibrator.getEstimatedMxy());
20797 assertNull(calibrator.getEstimatedMxz());
20798 assertNull(calibrator.getEstimatedMyx());
20799 assertNull(calibrator.getEstimatedMyz());
20800 assertNull(calibrator.getEstimatedMzx());
20801 assertNull(calibrator.getEstimatedMzy());
20802 assertNull(calibrator.getEstimatedCovariance());
20803 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20804 assertNotNull(calibrator.getGroundTruthGravityNorm());
20805 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20806 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20807 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20808 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20809 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20810 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20811 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20812
20813
20814 final Acceleration invalidGravityNorm = new Acceleration(
20815 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20816
20817 calibrator = null;
20818 try {
20819 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20820 invalidGravityNorm, true,
20821 bx, by, bz);
20822 fail("IllegalArgumentException expected but not thrown");
20823 } catch (final IllegalArgumentException ignore) {
20824 }
20825 assertNull(calibrator);
20826 }
20827
20828 @Test
20829 public void testConstructor178() throws WrongSizeException {
20830 final Matrix ba = generateBa();
20831 final double biasX = ba.getElementAtIndex(0);
20832 final double biasY = ba.getElementAtIndex(1);
20833 final double biasZ = ba.getElementAtIndex(2);
20834
20835 final Acceleration bx = new Acceleration(biasX,
20836 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20837 final Acceleration by = new Acceleration(biasY,
20838 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20839 final Acceleration bz = new Acceleration(biasZ,
20840 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20841
20842 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20843 final double latitude = Math.toRadians(
20844 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20845 final double longitude = Math.toRadians(
20846 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20847 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20848 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20849 final NEDVelocity nedVelocity = new NEDVelocity();
20850 final ECEFPosition ecefPosition = new ECEFPosition();
20851 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20852 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20853 ecefPosition, ecefVelocity);
20854 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20855 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20856 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20857
20858 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20859 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20860 true, bx, by, bz, this);
20861
20862
20863 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20864 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20865 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20866 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20867 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20868 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20869 final Acceleration bx2 = new Acceleration(0.0,
20870 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20871 calibrator.getBiasXAsAcceleration(bx2);
20872 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20873 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20874 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20875 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20876 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20877 final Acceleration by2 = new Acceleration(0.0,
20878 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20879 calibrator.getBiasYAsAcceleration(by2);
20880 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20881 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20882 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20883 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20884 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20885 final Acceleration bz2 = new Acceleration(0.0,
20886 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20887 calibrator.getBiasZAsAcceleration(bz2);
20888 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20889 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20890 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20891 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20892 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20893 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20894 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20895 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20896 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20897 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20898 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20899 final double[] bias1 = calibrator.getBias();
20900 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20901 final double[] bias2 = new double[3];
20902 calibrator.getBias(bias2);
20903 assertArrayEquals(bias1, bias2, 0.0);
20904 final Matrix b1 = calibrator.getBiasAsMatrix();
20905 assertEquals(b1, ba);
20906 final Matrix b2 = new Matrix(3, 1);
20907 calibrator.getBiasAsMatrix(b2);
20908 assertEquals(b1, b2);
20909 final Matrix ma1 = calibrator.getInitialMa();
20910 assertEquals(ma1, new Matrix(3, 3));
20911 final Matrix ma2 = new Matrix(3, 3);
20912 calibrator.getInitialMa(ma2);
20913 assertEquals(ma1, ma2);
20914 assertNull(calibrator.getMeasurements());
20915 assertTrue(calibrator.isCommonAxisUsed());
20916 assertSame(calibrator.getListener(), this);
20917 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20918 assertFalse(calibrator.isReady());
20919 assertFalse(calibrator.isRunning());
20920 assertNull(calibrator.getEstimatedMa());
20921 assertNull(calibrator.getEstimatedSx());
20922 assertNull(calibrator.getEstimatedSy());
20923 assertNull(calibrator.getEstimatedSz());
20924 assertNull(calibrator.getEstimatedMxy());
20925 assertNull(calibrator.getEstimatedMxz());
20926 assertNull(calibrator.getEstimatedMyx());
20927 assertNull(calibrator.getEstimatedMyz());
20928 assertNull(calibrator.getEstimatedMzx());
20929 assertNull(calibrator.getEstimatedMzy());
20930 assertNull(calibrator.getEstimatedCovariance());
20931 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20932 assertNotNull(calibrator.getGroundTruthGravityNorm());
20933 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20934 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20935 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20936 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20937 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20938 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20939 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20940
20941
20942 final Acceleration invalidGravityNorm = new Acceleration(
20943 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20944
20945 calibrator = null;
20946 try {
20947 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20948 invalidGravityNorm, true,
20949 bx, by, bz, this);
20950 fail("IllegalArgumentException expected but not thrown");
20951 } catch (final IllegalArgumentException ignore) {
20952 }
20953 assertNull(calibrator);
20954 }
20955
20956 @Test
20957 public void testConstructor179() throws WrongSizeException {
20958 final Collection<StandardDeviationBodyKinematics> measurements =
20959 Collections.emptyList();
20960
20961 final Matrix ba = generateBa();
20962 final double biasX = ba.getElementAtIndex(0);
20963 final double biasY = ba.getElementAtIndex(1);
20964 final double biasZ = ba.getElementAtIndex(2);
20965
20966 final Acceleration bx = new Acceleration(biasX,
20967 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20968 final Acceleration by = new Acceleration(biasY,
20969 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20970 final Acceleration bz = new Acceleration(biasZ,
20971 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20972
20973 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20974 final double latitude = Math.toRadians(
20975 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20976 final double longitude = Math.toRadians(
20977 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20978 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20979 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20980 final NEDVelocity nedVelocity = new NEDVelocity();
20981 final ECEFPosition ecefPosition = new ECEFPosition();
20982 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20983 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20984 ecefPosition, ecefVelocity);
20985 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20986 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20987 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20988
20989 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20990 new KnownBiasAndGravityNormAccelerometerCalibrator(
20991 gravityNorm, measurements,
20992 true, bx, by, bz);
20993
20994
20995 assertEquals(calibrator.getBiasX(), biasX, 0.0);
20996 assertEquals(calibrator.getBiasY(), biasY, 0.0);
20997 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20998 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20999 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21000 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21001 final Acceleration bx2 = new Acceleration(0.0,
21002 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21003 calibrator.getBiasXAsAcceleration(bx2);
21004 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21005 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21006 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21007 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21008 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21009 final Acceleration by2 = new Acceleration(0.0,
21010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21011 calibrator.getBiasYAsAcceleration(by2);
21012 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21013 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21014 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21015 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21016 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21017 final Acceleration bz2 = new Acceleration(0.0,
21018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21019 calibrator.getBiasZAsAcceleration(bz2);
21020 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21021 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21022 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21023 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21024 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21025 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21026 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21027 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21028 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21029 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21030 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21031 final double[] bias1 = calibrator.getBias();
21032 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21033 final double[] bias2 = new double[3];
21034 calibrator.getBias(bias2);
21035 assertArrayEquals(bias1, bias2, 0.0);
21036 final Matrix b1 = calibrator.getBiasAsMatrix();
21037 assertEquals(b1, ba);
21038 final Matrix b2 = new Matrix(3, 1);
21039 calibrator.getBiasAsMatrix(b2);
21040 assertEquals(b1, b2);
21041 final Matrix ma1 = calibrator.getInitialMa();
21042 assertEquals(ma1, new Matrix(3, 3));
21043 final Matrix ma2 = new Matrix(3, 3);
21044 calibrator.getInitialMa(ma2);
21045 assertEquals(ma1, ma2);
21046 assertSame(calibrator.getMeasurements(), measurements);
21047 assertTrue(calibrator.isCommonAxisUsed());
21048 assertNull(calibrator.getListener());
21049 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21050 assertFalse(calibrator.isReady());
21051 assertFalse(calibrator.isRunning());
21052 assertNull(calibrator.getEstimatedMa());
21053 assertNull(calibrator.getEstimatedSx());
21054 assertNull(calibrator.getEstimatedSy());
21055 assertNull(calibrator.getEstimatedSz());
21056 assertNull(calibrator.getEstimatedMxy());
21057 assertNull(calibrator.getEstimatedMxz());
21058 assertNull(calibrator.getEstimatedMyx());
21059 assertNull(calibrator.getEstimatedMyz());
21060 assertNull(calibrator.getEstimatedMzx());
21061 assertNull(calibrator.getEstimatedMzy());
21062 assertNull(calibrator.getEstimatedCovariance());
21063 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21064 assertNotNull(calibrator.getGroundTruthGravityNorm());
21065 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21066 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21067 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21068 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21069 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21070 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21071 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21072
21073
21074 final Acceleration invalidGravityNorm = new Acceleration(
21075 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21076
21077 calibrator = null;
21078 try {
21079 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21080 invalidGravityNorm, measurements,
21081 true, bx, by, bz);
21082 fail("IllegalArgumentException expected but not thrown");
21083 } catch (final IllegalArgumentException ignore) {
21084 }
21085 assertNull(calibrator);
21086 }
21087
21088 @Test
21089 public void testConstructor180() throws WrongSizeException {
21090 final Collection<StandardDeviationBodyKinematics> measurements =
21091 Collections.emptyList();
21092
21093 final Matrix ba = generateBa();
21094 final double biasX = ba.getElementAtIndex(0);
21095 final double biasY = ba.getElementAtIndex(1);
21096 final double biasZ = ba.getElementAtIndex(2);
21097
21098 final Acceleration bx = new Acceleration(biasX,
21099 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21100 final Acceleration by = new Acceleration(biasY,
21101 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21102 final Acceleration bz = new Acceleration(biasZ,
21103 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21104
21105 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21106 final double latitude = Math.toRadians(
21107 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21108 final double longitude = Math.toRadians(
21109 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21110 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21111 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21112 final NEDVelocity nedVelocity = new NEDVelocity();
21113 final ECEFPosition ecefPosition = new ECEFPosition();
21114 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21115 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21116 ecefPosition, ecefVelocity);
21117 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21118 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21119 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21120
21121 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21122 new KnownBiasAndGravityNormAccelerometerCalibrator(
21123 gravityNorm, measurements,
21124 true, bx, by, bz, this);
21125
21126
21127 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21128 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21129 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21130 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21131 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21132 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21133 final Acceleration bx2 = new Acceleration(0.0,
21134 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21135 calibrator.getBiasXAsAcceleration(bx2);
21136 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21137 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21138 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21139 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21140 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21141 final Acceleration by2 = new Acceleration(0.0,
21142 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21143 calibrator.getBiasYAsAcceleration(by2);
21144 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21145 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21146 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21147 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21148 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21149 final Acceleration bz2 = new Acceleration(0.0,
21150 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21151 calibrator.getBiasZAsAcceleration(bz2);
21152 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21153 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21154 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21155 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21156 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21157 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21158 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21159 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21160 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21161 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21162 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21163 final double[] bias1 = calibrator.getBias();
21164 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21165 final double[] bias2 = new double[3];
21166 calibrator.getBias(bias2);
21167 assertArrayEquals(bias1, bias2, 0.0);
21168 final Matrix b1 = calibrator.getBiasAsMatrix();
21169 assertEquals(b1, ba);
21170 final Matrix b2 = new Matrix(3, 1);
21171 calibrator.getBiasAsMatrix(b2);
21172 assertEquals(b1, b2);
21173 final Matrix ma1 = calibrator.getInitialMa();
21174 assertEquals(ma1, new Matrix(3, 3));
21175 final Matrix ma2 = new Matrix(3, 3);
21176 calibrator.getInitialMa(ma2);
21177 assertEquals(ma1, ma2);
21178 assertSame(calibrator.getMeasurements(), measurements);
21179 assertTrue(calibrator.isCommonAxisUsed());
21180 assertSame(calibrator.getListener(), this);
21181 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21182 assertFalse(calibrator.isReady());
21183 assertFalse(calibrator.isRunning());
21184 assertNull(calibrator.getEstimatedMa());
21185 assertNull(calibrator.getEstimatedSx());
21186 assertNull(calibrator.getEstimatedSy());
21187 assertNull(calibrator.getEstimatedSz());
21188 assertNull(calibrator.getEstimatedMxy());
21189 assertNull(calibrator.getEstimatedMxz());
21190 assertNull(calibrator.getEstimatedMyx());
21191 assertNull(calibrator.getEstimatedMyz());
21192 assertNull(calibrator.getEstimatedMzx());
21193 assertNull(calibrator.getEstimatedMzy());
21194 assertNull(calibrator.getEstimatedCovariance());
21195 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21196 assertNotNull(calibrator.getGroundTruthGravityNorm());
21197 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21198 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21199 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21200 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21201 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21202 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21203 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21204
21205
21206 final Acceleration invalidGravityNorm = new Acceleration(
21207 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21208
21209 calibrator = null;
21210 try {
21211 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21212 invalidGravityNorm, measurements, true,
21213 bx, by, bz, this);
21214 fail("IllegalArgumentException expected but not thrown");
21215 } catch (final IllegalArgumentException ignore) {
21216 }
21217 assertNull(calibrator);
21218 }
21219
21220 @Test
21221 public void testConstructor181() throws WrongSizeException {
21222 final Matrix ba = generateBa();
21223 final double biasX = ba.getElementAtIndex(0);
21224 final double biasY = ba.getElementAtIndex(1);
21225 final double biasZ = ba.getElementAtIndex(2);
21226
21227 final Matrix ma = generateMaCommonAxis();
21228 final double sx = ma.getElementAt(0, 0);
21229 final double sy = ma.getElementAt(1, 1);
21230 final double sz = ma.getElementAt(2, 2);
21231
21232 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21233 final double latitude = Math.toRadians(
21234 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21235 final double longitude = Math.toRadians(
21236 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21237 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21238 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21239 final NEDVelocity nedVelocity = new NEDVelocity();
21240 final ECEFPosition ecefPosition = new ECEFPosition();
21241 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21242 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21243 ecefPosition, ecefVelocity);
21244 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21245 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21246 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21247
21248 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21249 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21250 biasX, biasY, biasZ, sx, sy, sz);
21251
21252
21253 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21254 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21255 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21256 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21257 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21258 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21259 final Acceleration bx2 = new Acceleration(0.0,
21260 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21261 calibrator.getBiasXAsAcceleration(bx2);
21262 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21263 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21264 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21265 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21266 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21267 final Acceleration by2 = new Acceleration(0.0,
21268 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21269 calibrator.getBiasYAsAcceleration(by2);
21270 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21271 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21272 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21273 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21274 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21275 final Acceleration bz2 = new Acceleration(0.0,
21276 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21277 calibrator.getBiasZAsAcceleration(bz2);
21278 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21279 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21280 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21281 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21282 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21283 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21284 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21285 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21286 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21287 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21288 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21289 final double[] bias1 = calibrator.getBias();
21290 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21291 final double[] bias2 = new double[3];
21292 calibrator.getBias(bias2);
21293 assertArrayEquals(bias1, bias2, 0.0);
21294 final Matrix b1 = calibrator.getBiasAsMatrix();
21295 assertEquals(b1, ba);
21296 final Matrix b2 = new Matrix(3, 1);
21297 calibrator.getBiasAsMatrix(b2);
21298 assertEquals(b1, b2);
21299 final Matrix ma1 = calibrator.getInitialMa();
21300 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21301 final Matrix ma2 = new Matrix(3, 3);
21302 calibrator.getInitialMa(ma2);
21303 assertEquals(ma1, ma2);
21304 assertNull(calibrator.getMeasurements());
21305 assertFalse(calibrator.isCommonAxisUsed());
21306 assertNull(calibrator.getListener());
21307 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21308 assertFalse(calibrator.isReady());
21309 assertFalse(calibrator.isRunning());
21310 assertNull(calibrator.getEstimatedMa());
21311 assertNull(calibrator.getEstimatedSx());
21312 assertNull(calibrator.getEstimatedSy());
21313 assertNull(calibrator.getEstimatedSz());
21314 assertNull(calibrator.getEstimatedMxy());
21315 assertNull(calibrator.getEstimatedMxz());
21316 assertNull(calibrator.getEstimatedMyx());
21317 assertNull(calibrator.getEstimatedMyz());
21318 assertNull(calibrator.getEstimatedMzx());
21319 assertNull(calibrator.getEstimatedMzy());
21320 assertNull(calibrator.getEstimatedCovariance());
21321 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21322 assertNotNull(calibrator.getGroundTruthGravityNorm());
21323 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21324 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21325 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21326 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21327 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21328 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21329 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21330
21331
21332 final Acceleration invalidGravityNorm = new Acceleration(
21333 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21334
21335 calibrator = null;
21336 try {
21337 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21338 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz);
21339 fail("IllegalArgumentException expected but not thrown");
21340 } catch (final IllegalArgumentException ignore) {
21341 }
21342 assertNull(calibrator);
21343 }
21344
21345 @Test
21346 public void testConstructor182() throws WrongSizeException {
21347 final Collection<StandardDeviationBodyKinematics> measurements =
21348 Collections.emptyList();
21349
21350 final Matrix ba = generateBa();
21351 final double biasX = ba.getElementAtIndex(0);
21352 final double biasY = ba.getElementAtIndex(1);
21353 final double biasZ = ba.getElementAtIndex(2);
21354
21355 final Matrix ma = generateMaCommonAxis();
21356 final double sx = ma.getElementAt(0, 0);
21357 final double sy = ma.getElementAt(1, 1);
21358 final double sz = ma.getElementAt(2, 2);
21359
21360 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21361 final double latitude = Math.toRadians(
21362 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21363 final double longitude = Math.toRadians(
21364 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21365 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21366 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21367 final NEDVelocity nedVelocity = new NEDVelocity();
21368 final ECEFPosition ecefPosition = new ECEFPosition();
21369 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21370 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21371 ecefPosition, ecefVelocity);
21372 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21373 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21374 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21375
21376 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21377 new KnownBiasAndGravityNormAccelerometerCalibrator(
21378 gravityNorm, measurements,
21379 biasX, biasY, biasZ, sx, sy, sz);
21380
21381
21382 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21383 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21384 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21385 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21386 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21387 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21388 final Acceleration bx2 = new Acceleration(0.0,
21389 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21390 calibrator.getBiasXAsAcceleration(bx2);
21391 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21392 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21393 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21394 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21395 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21396 final Acceleration by2 = new Acceleration(0.0,
21397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21398 calibrator.getBiasYAsAcceleration(by2);
21399 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21400 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21401 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21402 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21403 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21404 final Acceleration bz2 = new Acceleration(0.0,
21405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21406 calibrator.getBiasZAsAcceleration(bz2);
21407 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21408 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21409 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21410 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21411 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21412 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21413 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21414 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21415 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21416 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21417 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21418 final double[] bias1 = calibrator.getBias();
21419 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21420 final double[] bias2 = new double[3];
21421 calibrator.getBias(bias2);
21422 assertArrayEquals(bias1, bias2, 0.0);
21423 final Matrix b1 = calibrator.getBiasAsMatrix();
21424 assertEquals(b1, ba);
21425 final Matrix b2 = new Matrix(3, 1);
21426 calibrator.getBiasAsMatrix(b2);
21427 assertEquals(b1, b2);
21428 final Matrix ma1 = calibrator.getInitialMa();
21429 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21430 final Matrix ma2 = new Matrix(3, 3);
21431 calibrator.getInitialMa(ma2);
21432 assertEquals(ma1, ma2);
21433 assertSame(calibrator.getMeasurements(), measurements);
21434 assertFalse(calibrator.isCommonAxisUsed());
21435 assertNull(calibrator.getListener());
21436 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21437 assertFalse(calibrator.isReady());
21438 assertFalse(calibrator.isRunning());
21439 assertNull(calibrator.getEstimatedMa());
21440 assertNull(calibrator.getEstimatedSx());
21441 assertNull(calibrator.getEstimatedSy());
21442 assertNull(calibrator.getEstimatedSz());
21443 assertNull(calibrator.getEstimatedMxy());
21444 assertNull(calibrator.getEstimatedMxz());
21445 assertNull(calibrator.getEstimatedMyx());
21446 assertNull(calibrator.getEstimatedMyz());
21447 assertNull(calibrator.getEstimatedMzx());
21448 assertNull(calibrator.getEstimatedMzy());
21449 assertNull(calibrator.getEstimatedCovariance());
21450 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21451 assertNotNull(calibrator.getGroundTruthGravityNorm());
21452 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21453 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21454 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21455 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21456 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21457 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21458 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21459
21460
21461 final Acceleration invalidGravityNorm = new Acceleration(
21462 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21463
21464 calibrator = null;
21465 try {
21466 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21467 invalidGravityNorm, measurements,
21468 biasX, biasY, biasZ, sx, sy, sz);
21469 fail("IllegalArgumentException expected but not thrown");
21470 } catch (final IllegalArgumentException ignore) {
21471 }
21472 assertNull(calibrator);
21473 }
21474
21475 @Test
21476 public void testConstructor183() throws WrongSizeException {
21477 final Collection<StandardDeviationBodyKinematics> measurements =
21478 Collections.emptyList();
21479
21480 final Matrix ba = generateBa();
21481 final double biasX = ba.getElementAtIndex(0);
21482 final double biasY = ba.getElementAtIndex(1);
21483 final double biasZ = ba.getElementAtIndex(2);
21484
21485 final Matrix ma = generateMaCommonAxis();
21486 final double sx = ma.getElementAt(0, 0);
21487 final double sy = ma.getElementAt(1, 1);
21488 final double sz = ma.getElementAt(2, 2);
21489
21490 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21491 final double latitude = Math.toRadians(
21492 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21493 final double longitude = Math.toRadians(
21494 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21495 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21496 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21497 final NEDVelocity nedVelocity = new NEDVelocity();
21498 final ECEFPosition ecefPosition = new ECEFPosition();
21499 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21500 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21501 ecefPosition, ecefVelocity);
21502 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21503 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21504 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21505
21506 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21507 new KnownBiasAndGravityNormAccelerometerCalibrator(
21508 gravityNorm, measurements,
21509 biasX, biasY, biasZ, sx, sy, sz, this);
21510
21511
21512 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21513 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21514 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21515 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21516 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21517 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21518 final Acceleration bx2 = new Acceleration(0.0,
21519 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21520 calibrator.getBiasXAsAcceleration(bx2);
21521 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21522 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21523 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21524 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21525 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21526 final Acceleration by2 = new Acceleration(0.0,
21527 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21528 calibrator.getBiasYAsAcceleration(by2);
21529 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21530 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21531 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21532 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21533 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21534 final Acceleration bz2 = new Acceleration(0.0,
21535 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21536 calibrator.getBiasZAsAcceleration(bz2);
21537 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21538 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21539 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21540 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21541 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21542 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21543 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21544 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21545 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21546 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21547 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21548 final double[] bias1 = calibrator.getBias();
21549 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21550 final double[] bias2 = new double[3];
21551 calibrator.getBias(bias2);
21552 assertArrayEquals(bias1, bias2, 0.0);
21553 final Matrix b1 = calibrator.getBiasAsMatrix();
21554 assertEquals(b1, ba);
21555 final Matrix b2 = new Matrix(3, 1);
21556 calibrator.getBiasAsMatrix(b2);
21557 assertEquals(b1, b2);
21558 final Matrix ma1 = calibrator.getInitialMa();
21559 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21560 final Matrix ma2 = new Matrix(3, 3);
21561 calibrator.getInitialMa(ma2);
21562 assertEquals(ma1, ma2);
21563 assertSame(calibrator.getMeasurements(), measurements);
21564 assertFalse(calibrator.isCommonAxisUsed());
21565 assertSame(calibrator.getListener(), this);
21566 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21567 assertFalse(calibrator.isReady());
21568 assertFalse(calibrator.isRunning());
21569 assertNull(calibrator.getEstimatedMa());
21570 assertNull(calibrator.getEstimatedSx());
21571 assertNull(calibrator.getEstimatedSy());
21572 assertNull(calibrator.getEstimatedSz());
21573 assertNull(calibrator.getEstimatedMxy());
21574 assertNull(calibrator.getEstimatedMxz());
21575 assertNull(calibrator.getEstimatedMyx());
21576 assertNull(calibrator.getEstimatedMyz());
21577 assertNull(calibrator.getEstimatedMzx());
21578 assertNull(calibrator.getEstimatedMzy());
21579 assertNull(calibrator.getEstimatedCovariance());
21580 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21581 assertNotNull(calibrator.getGroundTruthGravityNorm());
21582 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21583 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21584 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21585 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21586 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21587 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21588 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21589
21590
21591 final Acceleration invalidGravityNorm = new Acceleration(
21592 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21593
21594 calibrator = null;
21595 try {
21596 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21597 invalidGravityNorm, measurements,
21598 biasX, biasY, biasZ, sx, sy, sz, this);
21599 fail("IllegalArgumentException expected but not thrown");
21600 } catch (final IllegalArgumentException ignore) {
21601 }
21602 assertNull(calibrator);
21603 }
21604
21605 @Test
21606 public void testConstructor184() throws WrongSizeException {
21607 final Matrix ba = generateBa();
21608 final double biasX = ba.getElementAtIndex(0);
21609 final double biasY = ba.getElementAtIndex(1);
21610 final double biasZ = ba.getElementAtIndex(2);
21611
21612 final Matrix ma = generateMaCommonAxis();
21613 final double sx = ma.getElementAt(0, 0);
21614 final double sy = ma.getElementAt(1, 1);
21615 final double sz = ma.getElementAt(2, 2);
21616
21617 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21618 final double latitude = Math.toRadians(
21619 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21620 final double longitude = Math.toRadians(
21621 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21622 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21623 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21624 final NEDVelocity nedVelocity = new NEDVelocity();
21625 final ECEFPosition ecefPosition = new ECEFPosition();
21626 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21627 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21628 ecefPosition, ecefVelocity);
21629 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21630 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21631 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21632
21633 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21634 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21635 true, biasX, biasY, biasZ, sx, sy, sz);
21636
21637
21638 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21639 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21640 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21641 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21642 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21643 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21644 final Acceleration bx2 = new Acceleration(0.0,
21645 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21646 calibrator.getBiasXAsAcceleration(bx2);
21647 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21648 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21649 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21650 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21651 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21652 final Acceleration by2 = new Acceleration(0.0,
21653 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21654 calibrator.getBiasYAsAcceleration(by2);
21655 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21656 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21657 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21658 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21659 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21660 final Acceleration bz2 = new Acceleration(0.0,
21661 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21662 calibrator.getBiasZAsAcceleration(bz2);
21663 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21664 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21665 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21666 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21667 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21668 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21669 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21670 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21671 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21672 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21673 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21674 final double[] bias1 = calibrator.getBias();
21675 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21676 final double[] bias2 = new double[3];
21677 calibrator.getBias(bias2);
21678 assertArrayEquals(bias1, bias2, 0.0);
21679 final Matrix b1 = calibrator.getBiasAsMatrix();
21680 assertEquals(b1, ba);
21681 final Matrix b2 = new Matrix(3, 1);
21682 calibrator.getBiasAsMatrix(b2);
21683 assertEquals(b1, b2);
21684 final Matrix ma1 = calibrator.getInitialMa();
21685 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21686 final Matrix ma2 = new Matrix(3, 3);
21687 calibrator.getInitialMa(ma2);
21688 assertEquals(ma1, ma2);
21689 assertNull(calibrator.getMeasurements());
21690 assertTrue(calibrator.isCommonAxisUsed());
21691 assertNull(calibrator.getListener());
21692 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21693 assertFalse(calibrator.isReady());
21694 assertFalse(calibrator.isRunning());
21695 assertNull(calibrator.getEstimatedMa());
21696 assertNull(calibrator.getEstimatedSx());
21697 assertNull(calibrator.getEstimatedSy());
21698 assertNull(calibrator.getEstimatedSz());
21699 assertNull(calibrator.getEstimatedMxy());
21700 assertNull(calibrator.getEstimatedMxz());
21701 assertNull(calibrator.getEstimatedMyx());
21702 assertNull(calibrator.getEstimatedMyz());
21703 assertNull(calibrator.getEstimatedMzx());
21704 assertNull(calibrator.getEstimatedMzy());
21705 assertNull(calibrator.getEstimatedCovariance());
21706 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21707 assertNotNull(calibrator.getGroundTruthGravityNorm());
21708 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21709 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21710 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21711 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21712 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21713 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21714 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21715
21716
21717 final Acceleration invalidGravityNorm = new Acceleration(
21718 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21719
21720 calibrator = null;
21721 try {
21722 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21723 invalidGravityNorm, true, biasX, biasY, biasZ,
21724 sx, sy, sz);
21725 fail("IllegalArgumentException expected but not thrown");
21726 } catch (final IllegalArgumentException ignore) {
21727 }
21728 assertNull(calibrator);
21729 }
21730
21731 @Test
21732 public void testConstructor185() throws WrongSizeException {
21733 final Matrix ba = generateBa();
21734 final double biasX = ba.getElementAtIndex(0);
21735 final double biasY = ba.getElementAtIndex(1);
21736 final double biasZ = ba.getElementAtIndex(2);
21737
21738 final Matrix ma = generateMaCommonAxis();
21739 final double sx = ma.getElementAt(0, 0);
21740 final double sy = ma.getElementAt(1, 1);
21741 final double sz = ma.getElementAt(2, 2);
21742
21743 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21744 final double latitude = Math.toRadians(
21745 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21746 final double longitude = Math.toRadians(
21747 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21748 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21749 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21750 final NEDVelocity nedVelocity = new NEDVelocity();
21751 final ECEFPosition ecefPosition = new ECEFPosition();
21752 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21753 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21754 ecefPosition, ecefVelocity);
21755 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21756 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21757 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21758
21759 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21760 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21761 true, biasX, biasY, biasZ, sx, sy, sz,
21762 this);
21763
21764
21765 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21766 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21767 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21768 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21769 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21770 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21771 final Acceleration bx2 = new Acceleration(0.0,
21772 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21773 calibrator.getBiasXAsAcceleration(bx2);
21774 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21775 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21776 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21777 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21778 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21779 final Acceleration by2 = new Acceleration(0.0,
21780 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21781 calibrator.getBiasYAsAcceleration(by2);
21782 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21783 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21784 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21785 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21786 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21787 final Acceleration bz2 = new Acceleration(0.0,
21788 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21789 calibrator.getBiasZAsAcceleration(bz2);
21790 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21791 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21792 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21793 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21794 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21795 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21796 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21797 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21798 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21799 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21800 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21801 final double[] bias1 = calibrator.getBias();
21802 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21803 final double[] bias2 = new double[3];
21804 calibrator.getBias(bias2);
21805 assertArrayEquals(bias1, bias2, 0.0);
21806 final Matrix b1 = calibrator.getBiasAsMatrix();
21807 assertEquals(b1, ba);
21808 final Matrix b2 = new Matrix(3, 1);
21809 calibrator.getBiasAsMatrix(b2);
21810 assertEquals(b1, b2);
21811 final Matrix ma1 = calibrator.getInitialMa();
21812 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21813 final Matrix ma2 = new Matrix(3, 3);
21814 calibrator.getInitialMa(ma2);
21815 assertEquals(ma1, ma2);
21816 assertNull(calibrator.getMeasurements());
21817 assertTrue(calibrator.isCommonAxisUsed());
21818 assertSame(calibrator.getListener(), this);
21819 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21820 assertFalse(calibrator.isReady());
21821 assertFalse(calibrator.isRunning());
21822 assertNull(calibrator.getEstimatedMa());
21823 assertNull(calibrator.getEstimatedSx());
21824 assertNull(calibrator.getEstimatedSy());
21825 assertNull(calibrator.getEstimatedSz());
21826 assertNull(calibrator.getEstimatedMxy());
21827 assertNull(calibrator.getEstimatedMxz());
21828 assertNull(calibrator.getEstimatedMyx());
21829 assertNull(calibrator.getEstimatedMyz());
21830 assertNull(calibrator.getEstimatedMzx());
21831 assertNull(calibrator.getEstimatedMzy());
21832 assertNull(calibrator.getEstimatedCovariance());
21833 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21834 assertNotNull(calibrator.getGroundTruthGravityNorm());
21835 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21836 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21837 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21838 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21839 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21840 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21841 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21842
21843
21844 final Acceleration invalidGravityNorm = new Acceleration(
21845 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21846
21847 calibrator = null;
21848 try {
21849 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21850 invalidGravityNorm, true,
21851 biasX, biasY, biasZ,
21852 sx, sy, sz, this);
21853 fail("IllegalArgumentException expected but not thrown");
21854 } catch (final IllegalArgumentException ignore) {
21855 }
21856 assertNull(calibrator);
21857 }
21858
21859 @Test
21860 public void testConstructor186() throws WrongSizeException {
21861 final Collection<StandardDeviationBodyKinematics> measurements =
21862 Collections.emptyList();
21863
21864 final Matrix ba = generateBa();
21865 final double biasX = ba.getElementAtIndex(0);
21866 final double biasY = ba.getElementAtIndex(1);
21867 final double biasZ = ba.getElementAtIndex(2);
21868
21869 final Matrix ma = generateMaCommonAxis();
21870 final double sx = ma.getElementAt(0, 0);
21871 final double sy = ma.getElementAt(1, 1);
21872 final double sz = ma.getElementAt(2, 2);
21873
21874 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21875 final double latitude = Math.toRadians(
21876 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21877 final double longitude = Math.toRadians(
21878 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21879 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21880 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21881 final NEDVelocity nedVelocity = new NEDVelocity();
21882 final ECEFPosition ecefPosition = new ECEFPosition();
21883 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21884 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21885 ecefPosition, ecefVelocity);
21886 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21887 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21888 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21889
21890 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21891 new KnownBiasAndGravityNormAccelerometerCalibrator(
21892 gravityNorm, measurements,
21893 true, biasX, biasY, biasZ, sx, sy, sz);
21894
21895
21896 assertEquals(calibrator.getBiasX(), biasX, 0.0);
21897 assertEquals(calibrator.getBiasY(), biasY, 0.0);
21898 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21899 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21900 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21901 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902 final Acceleration bx2 = new Acceleration(0.0,
21903 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21904 calibrator.getBiasXAsAcceleration(bx2);
21905 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21906 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21907 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21908 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21909 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21910 final Acceleration by2 = new Acceleration(0.0,
21911 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21912 calibrator.getBiasYAsAcceleration(by2);
21913 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21914 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21915 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21916 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21917 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21918 final Acceleration bz2 = new Acceleration(0.0,
21919 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21920 calibrator.getBiasZAsAcceleration(bz2);
21921 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21922 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21923 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21924 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21925 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21926 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21927 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21928 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21929 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21930 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21931 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21932 final double[] bias1 = calibrator.getBias();
21933 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21934 final double[] bias2 = new double[3];
21935 calibrator.getBias(bias2);
21936 assertArrayEquals(bias1, bias2, 0.0);
21937 final Matrix b1 = calibrator.getBiasAsMatrix();
21938 assertEquals(b1, ba);
21939 final Matrix b2 = new Matrix(3, 1);
21940 calibrator.getBiasAsMatrix(b2);
21941 assertEquals(b1, b2);
21942 final Matrix ma1 = calibrator.getInitialMa();
21943 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21944 final Matrix ma2 = new Matrix(3, 3);
21945 calibrator.getInitialMa(ma2);
21946 assertEquals(ma1, ma2);
21947 assertSame(calibrator.getMeasurements(), measurements);
21948 assertTrue(calibrator.isCommonAxisUsed());
21949 assertNull(calibrator.getListener());
21950 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21951 assertFalse(calibrator.isReady());
21952 assertFalse(calibrator.isRunning());
21953 assertNull(calibrator.getEstimatedMa());
21954 assertNull(calibrator.getEstimatedSx());
21955 assertNull(calibrator.getEstimatedSy());
21956 assertNull(calibrator.getEstimatedSz());
21957 assertNull(calibrator.getEstimatedMxy());
21958 assertNull(calibrator.getEstimatedMxz());
21959 assertNull(calibrator.getEstimatedMyx());
21960 assertNull(calibrator.getEstimatedMyz());
21961 assertNull(calibrator.getEstimatedMzx());
21962 assertNull(calibrator.getEstimatedMzy());
21963 assertNull(calibrator.getEstimatedCovariance());
21964 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21965 assertNotNull(calibrator.getGroundTruthGravityNorm());
21966 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21967 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21968 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21969 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21970 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21971 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21972 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21973
21974
21975 final Acceleration invalidGravityNorm = new Acceleration(
21976 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21977
21978 calibrator = null;
21979 try {
21980 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21981 invalidGravityNorm, measurements,
21982 true, biasX, biasY, biasZ, sx, sy, sz);
21983 fail("IllegalArgumentException expected but not thrown");
21984 } catch (final IllegalArgumentException ignore) {
21985 }
21986 assertNull(calibrator);
21987 }
21988
21989 @Test
21990 public void testConstructor187() throws WrongSizeException {
21991 final Collection<StandardDeviationBodyKinematics> measurements =
21992 Collections.emptyList();
21993
21994 final Matrix ba = generateBa();
21995 final double biasX = ba.getElementAtIndex(0);
21996 final double biasY = ba.getElementAtIndex(1);
21997 final double biasZ = ba.getElementAtIndex(2);
21998
21999 final Matrix ma = generateMaCommonAxis();
22000 final double sx = ma.getElementAt(0, 0);
22001 final double sy = ma.getElementAt(1, 1);
22002 final double sz = ma.getElementAt(2, 2);
22003
22004 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22005 final double latitude = Math.toRadians(
22006 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22007 final double longitude = Math.toRadians(
22008 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22009 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22010 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22011 final NEDVelocity nedVelocity = new NEDVelocity();
22012 final ECEFPosition ecefPosition = new ECEFPosition();
22013 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22014 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22015 ecefPosition, ecefVelocity);
22016 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22017 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22018 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22019
22020 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22021 new KnownBiasAndGravityNormAccelerometerCalibrator(
22022 gravityNorm, measurements,
22023 true, biasX, biasY, biasZ, sx, sy, sz,
22024 this);
22025
22026
22027 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22028 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22029 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22030 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22031 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22032 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22033 final Acceleration bx2 = new Acceleration(0.0,
22034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22035 calibrator.getBiasXAsAcceleration(bx2);
22036 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22037 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22038 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22039 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22040 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22041 final Acceleration by2 = new Acceleration(0.0,
22042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22043 calibrator.getBiasYAsAcceleration(by2);
22044 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22045 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22047 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22048 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22049 final Acceleration bz2 = new Acceleration(0.0,
22050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22051 calibrator.getBiasZAsAcceleration(bz2);
22052 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22053 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22054 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22055 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22056 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22057 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22058 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22059 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22060 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22061 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22062 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22063 final double[] bias1 = calibrator.getBias();
22064 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22065 final double[] bias2 = new double[3];
22066 calibrator.getBias(bias2);
22067 assertArrayEquals(bias1, bias2, 0.0);
22068 final Matrix b1 = calibrator.getBiasAsMatrix();
22069 assertEquals(b1, ba);
22070 final Matrix b2 = new Matrix(3, 1);
22071 calibrator.getBiasAsMatrix(b2);
22072 assertEquals(b1, b2);
22073 final Matrix ma1 = calibrator.getInitialMa();
22074 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22075 final Matrix ma2 = new Matrix(3, 3);
22076 calibrator.getInitialMa(ma2);
22077 assertEquals(ma1, ma2);
22078 assertSame(calibrator.getMeasurements(), measurements);
22079 assertTrue(calibrator.isCommonAxisUsed());
22080 assertSame(calibrator.getListener(), this);
22081 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22082 assertFalse(calibrator.isReady());
22083 assertFalse(calibrator.isRunning());
22084 assertNull(calibrator.getEstimatedMa());
22085 assertNull(calibrator.getEstimatedSx());
22086 assertNull(calibrator.getEstimatedSy());
22087 assertNull(calibrator.getEstimatedSz());
22088 assertNull(calibrator.getEstimatedMxy());
22089 assertNull(calibrator.getEstimatedMxz());
22090 assertNull(calibrator.getEstimatedMyx());
22091 assertNull(calibrator.getEstimatedMyz());
22092 assertNull(calibrator.getEstimatedMzx());
22093 assertNull(calibrator.getEstimatedMzy());
22094 assertNull(calibrator.getEstimatedCovariance());
22095 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22096 assertNotNull(calibrator.getGroundTruthGravityNorm());
22097 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22098 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22099 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22100 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22101 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22102 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22103 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22104
22105
22106 final Acceleration invalidGravityNorm = new Acceleration(
22107 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22108
22109 calibrator = null;
22110 try {
22111 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22112 invalidGravityNorm, measurements,
22113 true, biasX, biasY, biasZ, sx, sy, sz,
22114 this);
22115 fail("IllegalArgumentException expected but not thrown");
22116 } catch (final IllegalArgumentException ignore) {
22117 }
22118 assertNull(calibrator);
22119 }
22120
22121 @Test
22122 public void testConstructor188() throws WrongSizeException {
22123 final Matrix ba = generateBa();
22124 final double biasX = ba.getElementAtIndex(0);
22125 final double biasY = ba.getElementAtIndex(1);
22126 final double biasZ = ba.getElementAtIndex(2);
22127
22128 final Acceleration bx = new Acceleration(biasX,
22129 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22130 final Acceleration by = new Acceleration(biasY,
22131 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22132 final Acceleration bz = new Acceleration(biasZ,
22133 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22134
22135 final Matrix ma = generateMaCommonAxis();
22136 final double sx = ma.getElementAt(0, 0);
22137 final double sy = ma.getElementAt(1, 1);
22138 final double sz = ma.getElementAt(2, 2);
22139
22140 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22141 final double latitude = Math.toRadians(
22142 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22143 final double longitude = Math.toRadians(
22144 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22145 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22146 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22147 final NEDVelocity nedVelocity = new NEDVelocity();
22148 final ECEFPosition ecefPosition = new ECEFPosition();
22149 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22150 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22151 ecefPosition, ecefVelocity);
22152 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22153 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22154 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22155
22156 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22157 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22158 bx, by, bz, sx, sy, sz);
22159
22160
22161 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22162 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22163 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22164 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22165 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22166 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22167 final Acceleration bx2 = new Acceleration(0.0,
22168 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22169 calibrator.getBiasXAsAcceleration(bx2);
22170 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22171 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22172 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22173 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22174 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22175 final Acceleration by2 = new Acceleration(0.0,
22176 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22177 calibrator.getBiasYAsAcceleration(by2);
22178 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22179 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22180 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22181 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22182 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22183 final Acceleration bz2 = new Acceleration(0.0,
22184 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22185 calibrator.getBiasZAsAcceleration(bz2);
22186 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22187 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22188 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22189 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22190 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22191 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22192 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22193 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22194 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22195 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22196 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22197 final double[] bias1 = calibrator.getBias();
22198 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22199 final double[] bias2 = new double[3];
22200 calibrator.getBias(bias2);
22201 assertArrayEquals(bias1, bias2, 0.0);
22202 final Matrix b1 = calibrator.getBiasAsMatrix();
22203 assertEquals(b1, ba);
22204 final Matrix b2 = new Matrix(3, 1);
22205 calibrator.getBiasAsMatrix(b2);
22206 assertEquals(b1, b2);
22207 final Matrix ma1 = calibrator.getInitialMa();
22208 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22209 final Matrix ma2 = new Matrix(3, 3);
22210 calibrator.getInitialMa(ma2);
22211 assertEquals(ma1, ma2);
22212 assertNull(calibrator.getMeasurements());
22213 assertFalse(calibrator.isCommonAxisUsed());
22214 assertNull(calibrator.getListener());
22215 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22216 assertFalse(calibrator.isReady());
22217 assertFalse(calibrator.isRunning());
22218 assertNull(calibrator.getEstimatedMa());
22219 assertNull(calibrator.getEstimatedSx());
22220 assertNull(calibrator.getEstimatedSy());
22221 assertNull(calibrator.getEstimatedSz());
22222 assertNull(calibrator.getEstimatedMxy());
22223 assertNull(calibrator.getEstimatedMxz());
22224 assertNull(calibrator.getEstimatedMyx());
22225 assertNull(calibrator.getEstimatedMyz());
22226 assertNull(calibrator.getEstimatedMzx());
22227 assertNull(calibrator.getEstimatedMzy());
22228 assertNull(calibrator.getEstimatedCovariance());
22229 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22230 assertNotNull(calibrator.getGroundTruthGravityNorm());
22231 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22232 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22233 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22234 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22235 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22236 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22237 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22238
22239
22240 final Acceleration invalidGravityNorm = new Acceleration(
22241 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22242
22243 calibrator = null;
22244 try {
22245 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22246 invalidGravityNorm,
22247 bx, by, bz, sx, sy, sz);
22248 fail("IllegalArgumentException expected but not thrown");
22249 } catch (final IllegalArgumentException ignore) {
22250 }
22251 assertNull(calibrator);
22252 }
22253
22254 @Test
22255 public void testConstructor189() throws WrongSizeException {
22256 final Matrix ba = generateBa();
22257 final double biasX = ba.getElementAtIndex(0);
22258 final double biasY = ba.getElementAtIndex(1);
22259 final double biasZ = ba.getElementAtIndex(2);
22260
22261 final Acceleration bx = new Acceleration(biasX,
22262 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22263 final Acceleration by = new Acceleration(biasY,
22264 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22265 final Acceleration bz = new Acceleration(biasZ,
22266 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22267
22268 final Matrix ma = generateMaCommonAxis();
22269 final double sx = ma.getElementAt(0, 0);
22270 final double sy = ma.getElementAt(1, 1);
22271 final double sz = ma.getElementAt(2, 2);
22272
22273 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22274 final double latitude = Math.toRadians(
22275 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22276 final double longitude = Math.toRadians(
22277 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22278 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22279 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22280 final NEDVelocity nedVelocity = new NEDVelocity();
22281 final ECEFPosition ecefPosition = new ECEFPosition();
22282 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22283 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22284 ecefPosition, ecefVelocity);
22285 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22286 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22287 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22288
22289 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22290 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22291 bx, by, bz, sx, sy, sz, this);
22292
22293
22294 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22295 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22296 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22297 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22298 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22299 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22300 final Acceleration bx2 = new Acceleration(0.0,
22301 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22302 calibrator.getBiasXAsAcceleration(bx2);
22303 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22304 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22305 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22306 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22307 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22308 final Acceleration by2 = new Acceleration(0.0,
22309 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22310 calibrator.getBiasYAsAcceleration(by2);
22311 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22312 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22313 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22314 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22315 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22316 final Acceleration bz2 = new Acceleration(0.0,
22317 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22318 calibrator.getBiasZAsAcceleration(bz2);
22319 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22320 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22321 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22322 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22323 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22324 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22325 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22326 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22327 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22328 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22329 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22330 final double[] bias1 = calibrator.getBias();
22331 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22332 final double[] bias2 = new double[3];
22333 calibrator.getBias(bias2);
22334 assertArrayEquals(bias1, bias2, 0.0);
22335 final Matrix b1 = calibrator.getBiasAsMatrix();
22336 assertEquals(b1, ba);
22337 final Matrix b2 = new Matrix(3, 1);
22338 calibrator.getBiasAsMatrix(b2);
22339 assertEquals(b1, b2);
22340 final Matrix ma1 = calibrator.getInitialMa();
22341 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22342 final Matrix ma2 = new Matrix(3, 3);
22343 calibrator.getInitialMa(ma2);
22344 assertEquals(ma1, ma2);
22345 assertNull(calibrator.getMeasurements());
22346 assertFalse(calibrator.isCommonAxisUsed());
22347 assertSame(calibrator.getListener(), this);
22348 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22349 assertFalse(calibrator.isReady());
22350 assertFalse(calibrator.isRunning());
22351 assertNull(calibrator.getEstimatedMa());
22352 assertNull(calibrator.getEstimatedSx());
22353 assertNull(calibrator.getEstimatedSy());
22354 assertNull(calibrator.getEstimatedSz());
22355 assertNull(calibrator.getEstimatedMxy());
22356 assertNull(calibrator.getEstimatedMxz());
22357 assertNull(calibrator.getEstimatedMyx());
22358 assertNull(calibrator.getEstimatedMyz());
22359 assertNull(calibrator.getEstimatedMzx());
22360 assertNull(calibrator.getEstimatedMzy());
22361 assertNull(calibrator.getEstimatedCovariance());
22362 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22363 assertNotNull(calibrator.getGroundTruthGravityNorm());
22364 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22365 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22366 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22367 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22368 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22369 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22370 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22371
22372
22373 final Acceleration invalidGravityNorm = new Acceleration(
22374 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22375
22376 calibrator = null;
22377 try {
22378 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22379 invalidGravityNorm, bx, by, bz, sx, sy, sz,
22380 this);
22381 fail("IllegalArgumentException expected but not thrown");
22382 } catch (final IllegalArgumentException ignore) {
22383 }
22384 assertNull(calibrator);
22385 }
22386
22387 @Test
22388 public void testConstructor190() throws WrongSizeException {
22389 final Collection<StandardDeviationBodyKinematics> measurements =
22390 Collections.emptyList();
22391
22392 final Matrix ba = generateBa();
22393 final double biasX = ba.getElementAtIndex(0);
22394 final double biasY = ba.getElementAtIndex(1);
22395 final double biasZ = ba.getElementAtIndex(2);
22396
22397 final Acceleration bx = new Acceleration(biasX,
22398 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22399 final Acceleration by = new Acceleration(biasY,
22400 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22401 final Acceleration bz = new Acceleration(biasZ,
22402 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22403
22404 final Matrix ma = generateMaCommonAxis();
22405 final double sx = ma.getElementAt(0, 0);
22406 final double sy = ma.getElementAt(1, 1);
22407 final double sz = ma.getElementAt(2, 2);
22408
22409 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22410 final double latitude = Math.toRadians(
22411 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22412 final double longitude = Math.toRadians(
22413 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22414 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22415 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22416 final NEDVelocity nedVelocity = new NEDVelocity();
22417 final ECEFPosition ecefPosition = new ECEFPosition();
22418 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22419 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22420 ecefPosition, ecefVelocity);
22421 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22422 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22423 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22424
22425 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22426 new KnownBiasAndGravityNormAccelerometerCalibrator(
22427 gravityNorm, measurements,
22428 bx, by, bz, sx, sy, sz);
22429
22430
22431 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22432 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22433 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22434 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22435 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22436 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22437 final Acceleration bx2 = new Acceleration(0.0,
22438 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22439 calibrator.getBiasXAsAcceleration(bx2);
22440 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22441 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22442 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22443 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22444 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22445 final Acceleration by2 = new Acceleration(0.0,
22446 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22447 calibrator.getBiasYAsAcceleration(by2);
22448 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22449 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22450 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22451 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22452 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22453 final Acceleration bz2 = new Acceleration(0.0,
22454 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22455 calibrator.getBiasZAsAcceleration(bz2);
22456 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22457 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22458 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22459 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22460 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22461 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22462 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22463 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22464 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22465 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22466 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22467 final double[] bias1 = calibrator.getBias();
22468 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22469 final double[] bias2 = new double[3];
22470 calibrator.getBias(bias2);
22471 assertArrayEquals(bias1, bias2, 0.0);
22472 final Matrix b1 = calibrator.getBiasAsMatrix();
22473 assertEquals(b1, ba);
22474 final Matrix b2 = new Matrix(3, 1);
22475 calibrator.getBiasAsMatrix(b2);
22476 assertEquals(b1, b2);
22477 final Matrix ma1 = calibrator.getInitialMa();
22478 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22479 final Matrix ma2 = new Matrix(3, 3);
22480 calibrator.getInitialMa(ma2);
22481 assertEquals(ma1, ma2);
22482 assertSame(calibrator.getMeasurements(), measurements);
22483 assertFalse(calibrator.isCommonAxisUsed());
22484 assertNull(calibrator.getListener());
22485 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22486 assertFalse(calibrator.isReady());
22487 assertFalse(calibrator.isRunning());
22488 assertNull(calibrator.getEstimatedMa());
22489 assertNull(calibrator.getEstimatedSx());
22490 assertNull(calibrator.getEstimatedSy());
22491 assertNull(calibrator.getEstimatedSz());
22492 assertNull(calibrator.getEstimatedMxy());
22493 assertNull(calibrator.getEstimatedMxz());
22494 assertNull(calibrator.getEstimatedMyx());
22495 assertNull(calibrator.getEstimatedMyz());
22496 assertNull(calibrator.getEstimatedMzx());
22497 assertNull(calibrator.getEstimatedMzy());
22498 assertNull(calibrator.getEstimatedCovariance());
22499 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22500 assertNotNull(calibrator.getGroundTruthGravityNorm());
22501 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22502 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22503 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22504 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22505 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22506 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22507 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22508
22509
22510 final Acceleration invalidGravityNorm = new Acceleration(
22511 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22512
22513 calibrator = null;
22514 try {
22515 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22516 invalidGravityNorm, measurements,
22517 bx, by, bz, sx, sy, sz);
22518 fail("IllegalArgumentException expected but not thrown");
22519 } catch (final IllegalArgumentException ignore) {
22520 }
22521 assertNull(calibrator);
22522 }
22523
22524 @Test
22525 public void testConstructor191() throws WrongSizeException {
22526 final Collection<StandardDeviationBodyKinematics> measurements =
22527 Collections.emptyList();
22528
22529 final Matrix ba = generateBa();
22530 final double biasX = ba.getElementAtIndex(0);
22531 final double biasY = ba.getElementAtIndex(1);
22532 final double biasZ = ba.getElementAtIndex(2);
22533
22534 final Acceleration bx = new Acceleration(biasX,
22535 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22536 final Acceleration by = new Acceleration(biasY,
22537 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22538 final Acceleration bz = new Acceleration(biasZ,
22539 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22540
22541 final Matrix ma = generateMaCommonAxis();
22542 final double sx = ma.getElementAt(0, 0);
22543 final double sy = ma.getElementAt(1, 1);
22544 final double sz = ma.getElementAt(2, 2);
22545
22546 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22547 final double latitude = Math.toRadians(
22548 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22549 final double longitude = Math.toRadians(
22550 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22551 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22552 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22553 final NEDVelocity nedVelocity = new NEDVelocity();
22554 final ECEFPosition ecefPosition = new ECEFPosition();
22555 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22556 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22557 ecefPosition, ecefVelocity);
22558 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22559 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22560 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22561
22562 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22563 new KnownBiasAndGravityNormAccelerometerCalibrator(
22564 gravityNorm, measurements,
22565 bx, by, bz, sx, sy, sz, this);
22566
22567
22568 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22569 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22570 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22571 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22572 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22573 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22574 final Acceleration bx2 = new Acceleration(0.0,
22575 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22576 calibrator.getBiasXAsAcceleration(bx2);
22577 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22578 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22579 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22580 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22581 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22582 final Acceleration by2 = new Acceleration(0.0,
22583 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22584 calibrator.getBiasYAsAcceleration(by2);
22585 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22586 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22587 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22588 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22589 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22590 final Acceleration bz2 = new Acceleration(0.0,
22591 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22592 calibrator.getBiasZAsAcceleration(bz2);
22593 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22594 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22595 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22596 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22597 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22598 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22599 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22600 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22601 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22602 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22603 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22604 final double[] bias1 = calibrator.getBias();
22605 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22606 final double[] bias2 = new double[3];
22607 calibrator.getBias(bias2);
22608 assertArrayEquals(bias1, bias2, 0.0);
22609 final Matrix b1 = calibrator.getBiasAsMatrix();
22610 assertEquals(b1, ba);
22611 final Matrix b2 = new Matrix(3, 1);
22612 calibrator.getBiasAsMatrix(b2);
22613 assertEquals(b1, b2);
22614 final Matrix ma1 = calibrator.getInitialMa();
22615 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22616 final Matrix ma2 = new Matrix(3, 3);
22617 calibrator.getInitialMa(ma2);
22618 assertEquals(ma1, ma2);
22619 assertSame(calibrator.getMeasurements(), measurements);
22620 assertFalse(calibrator.isCommonAxisUsed());
22621 assertSame(calibrator.getListener(), this);
22622 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22623 assertFalse(calibrator.isReady());
22624 assertFalse(calibrator.isRunning());
22625 assertNull(calibrator.getEstimatedMa());
22626 assertNull(calibrator.getEstimatedSx());
22627 assertNull(calibrator.getEstimatedSy());
22628 assertNull(calibrator.getEstimatedSz());
22629 assertNull(calibrator.getEstimatedMxy());
22630 assertNull(calibrator.getEstimatedMxz());
22631 assertNull(calibrator.getEstimatedMyx());
22632 assertNull(calibrator.getEstimatedMyz());
22633 assertNull(calibrator.getEstimatedMzx());
22634 assertNull(calibrator.getEstimatedMzy());
22635 assertNull(calibrator.getEstimatedCovariance());
22636 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22637 assertNotNull(calibrator.getGroundTruthGravityNorm());
22638 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22639 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22640 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22641 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22642 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22643 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22644 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22645
22646
22647 final Acceleration invalidGravityNorm = new Acceleration(
22648 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22649
22650 calibrator = null;
22651 try {
22652 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22653 invalidGravityNorm, measurements,
22654 bx, by, bz, sx, sy, sz, this);
22655 fail("IllegalArgumentException expected but not thrown");
22656 } catch (final IllegalArgumentException ignore) {
22657 }
22658 assertNull(calibrator);
22659 }
22660
22661 @Test
22662 public void testConstructor192() throws WrongSizeException {
22663 final Matrix ba = generateBa();
22664 final double biasX = ba.getElementAtIndex(0);
22665 final double biasY = ba.getElementAtIndex(1);
22666 final double biasZ = ba.getElementAtIndex(2);
22667
22668 final Acceleration bx = new Acceleration(biasX,
22669 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22670 final Acceleration by = new Acceleration(biasY,
22671 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22672 final Acceleration bz = new Acceleration(biasZ,
22673 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22674
22675 final Matrix ma = generateMaCommonAxis();
22676 final double sx = ma.getElementAt(0, 0);
22677 final double sy = ma.getElementAt(1, 1);
22678 final double sz = ma.getElementAt(2, 2);
22679
22680 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22681 final double latitude = Math.toRadians(
22682 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22683 final double longitude = Math.toRadians(
22684 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22685 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22686 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22687 final NEDVelocity nedVelocity = new NEDVelocity();
22688 final ECEFPosition ecefPosition = new ECEFPosition();
22689 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22690 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22691 ecefPosition, ecefVelocity);
22692 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22693 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22694 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22695
22696 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22697 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22698 true, bx, by, bz, sx, sy, sz);
22699
22700
22701 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22702 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22703 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22704 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22705 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22706 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22707 final Acceleration bx2 = new Acceleration(0.0,
22708 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22709 calibrator.getBiasXAsAcceleration(bx2);
22710 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22711 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22712 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22713 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22714 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22715 final Acceleration by2 = new Acceleration(0.0,
22716 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22717 calibrator.getBiasYAsAcceleration(by2);
22718 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22719 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22720 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22721 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22722 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22723 final Acceleration bz2 = new Acceleration(0.0,
22724 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22725 calibrator.getBiasZAsAcceleration(bz2);
22726 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22727 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22728 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22729 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22730 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22731 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22732 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22733 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22734 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22735 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22736 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22737 final double[] bias1 = calibrator.getBias();
22738 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22739 final double[] bias2 = new double[3];
22740 calibrator.getBias(bias2);
22741 assertArrayEquals(bias1, bias2, 0.0);
22742 final Matrix b1 = calibrator.getBiasAsMatrix();
22743 assertEquals(b1, ba);
22744 final Matrix b2 = new Matrix(3, 1);
22745 calibrator.getBiasAsMatrix(b2);
22746 assertEquals(b1, b2);
22747 final Matrix ma1 = calibrator.getInitialMa();
22748 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22749 final Matrix ma2 = new Matrix(3, 3);
22750 calibrator.getInitialMa(ma2);
22751 assertEquals(ma1, ma2);
22752 assertNull(calibrator.getMeasurements());
22753 assertTrue(calibrator.isCommonAxisUsed());
22754 assertNull(calibrator.getListener());
22755 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22756 assertFalse(calibrator.isReady());
22757 assertFalse(calibrator.isRunning());
22758 assertNull(calibrator.getEstimatedMa());
22759 assertNull(calibrator.getEstimatedSx());
22760 assertNull(calibrator.getEstimatedSy());
22761 assertNull(calibrator.getEstimatedSz());
22762 assertNull(calibrator.getEstimatedMxy());
22763 assertNull(calibrator.getEstimatedMxz());
22764 assertNull(calibrator.getEstimatedMyx());
22765 assertNull(calibrator.getEstimatedMyz());
22766 assertNull(calibrator.getEstimatedMzx());
22767 assertNull(calibrator.getEstimatedMzy());
22768 assertNull(calibrator.getEstimatedCovariance());
22769 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22770 assertNotNull(calibrator.getGroundTruthGravityNorm());
22771 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22772 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22773 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22774 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22775 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22776 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22777 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22778
22779
22780 final Acceleration invalidGravityNorm = new Acceleration(
22781 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782
22783 calibrator = null;
22784 try {
22785 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22786 invalidGravityNorm, true,
22787 bx, by, bz, sx, sy, sz);
22788 fail("IllegalArgumentException expected but not thrown");
22789 } catch (final IllegalArgumentException ignore) {
22790 }
22791 assertNull(calibrator);
22792 }
22793
22794 @Test
22795 public void testConstructor193() throws WrongSizeException {
22796 final Matrix ba = generateBa();
22797 final double biasX = ba.getElementAtIndex(0);
22798 final double biasY = ba.getElementAtIndex(1);
22799 final double biasZ = ba.getElementAtIndex(2);
22800
22801 final Acceleration bx = new Acceleration(biasX,
22802 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22803 final Acceleration by = new Acceleration(biasY,
22804 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22805 final Acceleration bz = new Acceleration(biasZ,
22806 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22807
22808 final Matrix ma = generateMaCommonAxis();
22809 final double sx = ma.getElementAt(0, 0);
22810 final double sy = ma.getElementAt(1, 1);
22811 final double sz = ma.getElementAt(2, 2);
22812
22813 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22814 final double latitude = Math.toRadians(
22815 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22816 final double longitude = Math.toRadians(
22817 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22818 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22819 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22820 final NEDVelocity nedVelocity = new NEDVelocity();
22821 final ECEFPosition ecefPosition = new ECEFPosition();
22822 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22823 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22824 ecefPosition, ecefVelocity);
22825 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22826 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22827 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22828
22829 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22830 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22831 true, bx, by, bz, sx, sy, sz, this);
22832
22833
22834 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22835 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22836 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22837 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22838 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22839 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22840 final Acceleration bx2 = new Acceleration(0.0,
22841 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22842 calibrator.getBiasXAsAcceleration(bx2);
22843 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22844 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22845 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22846 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22847 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22848 final Acceleration by2 = new Acceleration(0.0,
22849 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22850 calibrator.getBiasYAsAcceleration(by2);
22851 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22852 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22853 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22854 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22855 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22856 final Acceleration bz2 = new Acceleration(0.0,
22857 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22858 calibrator.getBiasZAsAcceleration(bz2);
22859 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22860 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22861 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22862 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22863 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22864 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22865 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22866 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22867 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22868 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22869 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22870 final double[] bias1 = calibrator.getBias();
22871 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22872 final double[] bias2 = new double[3];
22873 calibrator.getBias(bias2);
22874 assertArrayEquals(bias1, bias2, 0.0);
22875 final Matrix b1 = calibrator.getBiasAsMatrix();
22876 assertEquals(b1, ba);
22877 final Matrix b2 = new Matrix(3, 1);
22878 calibrator.getBiasAsMatrix(b2);
22879 assertEquals(b1, b2);
22880 final Matrix ma1 = calibrator.getInitialMa();
22881 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22882 final Matrix ma2 = new Matrix(3, 3);
22883 calibrator.getInitialMa(ma2);
22884 assertEquals(ma1, ma2);
22885 assertNull(calibrator.getMeasurements());
22886 assertTrue(calibrator.isCommonAxisUsed());
22887 assertSame(calibrator.getListener(), this);
22888 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22889 assertFalse(calibrator.isReady());
22890 assertFalse(calibrator.isRunning());
22891 assertNull(calibrator.getEstimatedMa());
22892 assertNull(calibrator.getEstimatedSx());
22893 assertNull(calibrator.getEstimatedSy());
22894 assertNull(calibrator.getEstimatedSz());
22895 assertNull(calibrator.getEstimatedMxy());
22896 assertNull(calibrator.getEstimatedMxz());
22897 assertNull(calibrator.getEstimatedMyx());
22898 assertNull(calibrator.getEstimatedMyz());
22899 assertNull(calibrator.getEstimatedMzx());
22900 assertNull(calibrator.getEstimatedMzy());
22901 assertNull(calibrator.getEstimatedCovariance());
22902 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22903 assertNotNull(calibrator.getGroundTruthGravityNorm());
22904 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22905 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22906 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22907 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22908 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22909 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22910 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22911
22912
22913 final Acceleration invalidGravityNorm = new Acceleration(
22914 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22915
22916 calibrator = null;
22917 try {
22918 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22919 invalidGravityNorm, true,
22920 bx, by, bz, sx, sy, sz, this);
22921 fail("IllegalArgumentException expected but not thrown");
22922 } catch (final IllegalArgumentException ignore) {
22923 }
22924 assertNull(calibrator);
22925 }
22926
22927 @Test
22928 public void testConstructor194() throws WrongSizeException {
22929 final Collection<StandardDeviationBodyKinematics> measurements =
22930 Collections.emptyList();
22931
22932 final Matrix ba = generateBa();
22933 final double biasX = ba.getElementAtIndex(0);
22934 final double biasY = ba.getElementAtIndex(1);
22935 final double biasZ = ba.getElementAtIndex(2);
22936
22937 final Acceleration bx = new Acceleration(biasX,
22938 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22939 final Acceleration by = new Acceleration(biasY,
22940 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22941 final Acceleration bz = new Acceleration(biasZ,
22942 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22943
22944 final Matrix ma = generateMaCommonAxis();
22945 final double sx = ma.getElementAt(0, 0);
22946 final double sy = ma.getElementAt(1, 1);
22947 final double sz = ma.getElementAt(2, 2);
22948
22949 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22950 final double latitude = Math.toRadians(
22951 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22952 final double longitude = Math.toRadians(
22953 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22954 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22955 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22956 final NEDVelocity nedVelocity = new NEDVelocity();
22957 final ECEFPosition ecefPosition = new ECEFPosition();
22958 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22959 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22960 ecefPosition, ecefVelocity);
22961 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22962 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22963 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22964
22965 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22966 new KnownBiasAndGravityNormAccelerometerCalibrator(
22967 gravityNorm, measurements,
22968 true, bx, by, bz, sx, sy, sz);
22969
22970
22971 assertEquals(calibrator.getBiasX(), biasX, 0.0);
22972 assertEquals(calibrator.getBiasY(), biasY, 0.0);
22973 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22974 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22975 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22976 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22977 final Acceleration bx2 = new Acceleration(0.0,
22978 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22979 calibrator.getBiasXAsAcceleration(bx2);
22980 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22981 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22982 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22983 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22984 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22985 final Acceleration by2 = new Acceleration(0.0,
22986 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22987 calibrator.getBiasYAsAcceleration(by2);
22988 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22989 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22990 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22991 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22992 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22993 final Acceleration bz2 = new Acceleration(0.0,
22994 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22995 calibrator.getBiasZAsAcceleration(bz2);
22996 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22997 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22998 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22999 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23000 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23001 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23002 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23003 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23004 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23005 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23006 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23007 final double[] bias1 = calibrator.getBias();
23008 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23009 final double[] bias2 = new double[3];
23010 calibrator.getBias(bias2);
23011 assertArrayEquals(bias1, bias2, 0.0);
23012 final Matrix b1 = calibrator.getBiasAsMatrix();
23013 assertEquals(b1, ba);
23014 final Matrix b2 = new Matrix(3, 1);
23015 calibrator.getBiasAsMatrix(b2);
23016 assertEquals(b1, b2);
23017 final Matrix ma1 = calibrator.getInitialMa();
23018 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23019 final Matrix ma2 = new Matrix(3, 3);
23020 calibrator.getInitialMa(ma2);
23021 assertEquals(ma1, ma2);
23022 assertSame(calibrator.getMeasurements(), measurements);
23023 assertTrue(calibrator.isCommonAxisUsed());
23024 assertNull(calibrator.getListener());
23025 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23026 assertFalse(calibrator.isReady());
23027 assertFalse(calibrator.isRunning());
23028 assertNull(calibrator.getEstimatedMa());
23029 assertNull(calibrator.getEstimatedSx());
23030 assertNull(calibrator.getEstimatedSy());
23031 assertNull(calibrator.getEstimatedSz());
23032 assertNull(calibrator.getEstimatedMxy());
23033 assertNull(calibrator.getEstimatedMxz());
23034 assertNull(calibrator.getEstimatedMyx());
23035 assertNull(calibrator.getEstimatedMyz());
23036 assertNull(calibrator.getEstimatedMzx());
23037 assertNull(calibrator.getEstimatedMzy());
23038 assertNull(calibrator.getEstimatedCovariance());
23039 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23040 assertNotNull(calibrator.getGroundTruthGravityNorm());
23041 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23042 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23043 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23044 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23045 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23046 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23047 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23048
23049
23050 final Acceleration invalidGravityNorm = new Acceleration(
23051 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23052
23053 calibrator = null;
23054 try {
23055 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23056 invalidGravityNorm, measurements,
23057 true, bx, by, bz, sx, sy, sz);
23058 fail("IllegalArgumentException expected but not thrown");
23059 } catch (final IllegalArgumentException ignore) {
23060 }
23061 assertNull(calibrator);
23062 }
23063
23064 @Test
23065 public void testConstructor195() throws WrongSizeException {
23066 final Collection<StandardDeviationBodyKinematics> measurements =
23067 Collections.emptyList();
23068
23069 final Matrix ba = generateBa();
23070 final double biasX = ba.getElementAtIndex(0);
23071 final double biasY = ba.getElementAtIndex(1);
23072 final double biasZ = ba.getElementAtIndex(2);
23073
23074 final Acceleration bx = new Acceleration(biasX,
23075 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23076 final Acceleration by = new Acceleration(biasY,
23077 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23078 final Acceleration bz = new Acceleration(biasZ,
23079 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23080
23081 final Matrix ma = generateMaCommonAxis();
23082 final double sx = ma.getElementAt(0, 0);
23083 final double sy = ma.getElementAt(1, 1);
23084 final double sz = ma.getElementAt(2, 2);
23085
23086 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23087 final double latitude = Math.toRadians(
23088 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23089 final double longitude = Math.toRadians(
23090 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23091 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23092 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23093 final NEDVelocity nedVelocity = new NEDVelocity();
23094 final ECEFPosition ecefPosition = new ECEFPosition();
23095 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23096 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23097 ecefPosition, ecefVelocity);
23098 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23099 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23100 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23101
23102 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23103 new KnownBiasAndGravityNormAccelerometerCalibrator(
23104 gravityNorm, measurements,
23105 true, bx, by, bz, sx, sy, sz, this);
23106
23107
23108 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23109 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23110 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23111 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23112 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23113 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23114 final Acceleration bx2 = new Acceleration(0.0,
23115 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23116 calibrator.getBiasXAsAcceleration(bx2);
23117 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23118 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23119 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23120 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23121 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23122 final Acceleration by2 = new Acceleration(0.0,
23123 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23124 calibrator.getBiasYAsAcceleration(by2);
23125 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23126 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23127 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23128 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23129 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23130 final Acceleration bz2 = new Acceleration(0.0,
23131 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23132 calibrator.getBiasZAsAcceleration(bz2);
23133 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23134 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23135 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23136 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23137 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23138 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23139 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23140 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23141 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23142 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23143 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23144 final double[] bias1 = calibrator.getBias();
23145 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23146 final double[] bias2 = new double[3];
23147 calibrator.getBias(bias2);
23148 assertArrayEquals(bias1, bias2, 0.0);
23149 final Matrix b1 = calibrator.getBiasAsMatrix();
23150 assertEquals(b1, ba);
23151 final Matrix b2 = new Matrix(3, 1);
23152 calibrator.getBiasAsMatrix(b2);
23153 assertEquals(b1, b2);
23154 final Matrix ma1 = calibrator.getInitialMa();
23155 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23156 final Matrix ma2 = new Matrix(3, 3);
23157 calibrator.getInitialMa(ma2);
23158 assertEquals(ma1, ma2);
23159 assertSame(calibrator.getMeasurements(), measurements);
23160 assertTrue(calibrator.isCommonAxisUsed());
23161 assertSame(calibrator.getListener(), this);
23162 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23163 assertFalse(calibrator.isReady());
23164 assertFalse(calibrator.isRunning());
23165 assertNull(calibrator.getEstimatedMa());
23166 assertNull(calibrator.getEstimatedSx());
23167 assertNull(calibrator.getEstimatedSy());
23168 assertNull(calibrator.getEstimatedSz());
23169 assertNull(calibrator.getEstimatedMxy());
23170 assertNull(calibrator.getEstimatedMxz());
23171 assertNull(calibrator.getEstimatedMyx());
23172 assertNull(calibrator.getEstimatedMyz());
23173 assertNull(calibrator.getEstimatedMzx());
23174 assertNull(calibrator.getEstimatedMzy());
23175 assertNull(calibrator.getEstimatedCovariance());
23176 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23177 assertNotNull(calibrator.getGroundTruthGravityNorm());
23178 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23179 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23180 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23181 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23182 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23183 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23184 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23185
23186
23187 final Acceleration invalidGravityNorm = new Acceleration(
23188 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23189
23190 calibrator = null;
23191 try {
23192 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23193 invalidGravityNorm, measurements,
23194 true, bx, by, bz, sx, sy, sz, this);
23195 fail("IllegalArgumentException expected but not thrown");
23196 } catch (final IllegalArgumentException ignore) {
23197 }
23198 assertNull(calibrator);
23199 }
23200
23201 @Test
23202 public void testConstructor196() throws WrongSizeException {
23203 final Matrix ba = generateBa();
23204 final double biasX = ba.getElementAtIndex(0);
23205 final double biasY = ba.getElementAtIndex(1);
23206 final double biasZ = ba.getElementAtIndex(2);
23207
23208 final Matrix ma = generateMaCommonAxis();
23209 final double sx = ma.getElementAt(0, 0);
23210 final double sy = ma.getElementAt(1, 1);
23211 final double sz = ma.getElementAt(2, 2);
23212 final double mxy = ma.getElementAt(0, 1);
23213 final double mxz = ma.getElementAt(0, 2);
23214 final double myx = ma.getElementAt(1, 0);
23215 final double myz = ma.getElementAt(1, 2);
23216 final double mzx = ma.getElementAt(2, 0);
23217 final double mzy = ma.getElementAt(2, 1);
23218
23219 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23220 final double latitude = Math.toRadians(
23221 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23222 final double longitude = Math.toRadians(
23223 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23224 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23225 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23226 final NEDVelocity nedVelocity = new NEDVelocity();
23227 final ECEFPosition ecefPosition = new ECEFPosition();
23228 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23229 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23230 ecefPosition, ecefVelocity);
23231 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23232 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23233 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23234
23235 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23236 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23237 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23238 myx, myz, mzx, mzy);
23239
23240
23241 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23242 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23243 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23244 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23245 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23246 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23247 final Acceleration bx2 = new Acceleration(0.0,
23248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23249 calibrator.getBiasXAsAcceleration(bx2);
23250 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23251 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23252 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23253 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23254 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23255 final Acceleration by2 = new Acceleration(0.0,
23256 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23257 calibrator.getBiasYAsAcceleration(by2);
23258 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23259 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23260 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23261 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23262 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23263 final Acceleration bz2 = new Acceleration(0.0,
23264 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23265 calibrator.getBiasZAsAcceleration(bz2);
23266 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23267 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23268 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23269 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23270 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23271 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23272 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23273 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23274 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23275 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23276 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23277 final double[] bias1 = calibrator.getBias();
23278 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23279 final double[] bias2 = new double[3];
23280 calibrator.getBias(bias2);
23281 assertArrayEquals(bias1, bias2, 0.0);
23282 final Matrix b1 = calibrator.getBiasAsMatrix();
23283 assertEquals(b1, ba);
23284 final Matrix b2 = new Matrix(3, 1);
23285 calibrator.getBiasAsMatrix(b2);
23286 assertEquals(b1, b2);
23287 final Matrix ma1 = new Matrix(3, 3);
23288 ma1.setSubmatrix(0, 0,
23289 2, 2,
23290 new double[]{sx, myx, mzx,
23291 mxy, sy, mzy,
23292 mxz, myz, sz});
23293 assertEquals(calibrator.getInitialMa(), ma1);
23294 final Matrix ma2 = new Matrix(3, 3);
23295 calibrator.getInitialMa(ma2);
23296 assertEquals(ma1, ma2);
23297 assertNull(calibrator.getMeasurements());
23298 assertFalse(calibrator.isCommonAxisUsed());
23299 assertNull(calibrator.getListener());
23300 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23301 assertFalse(calibrator.isReady());
23302 assertFalse(calibrator.isRunning());
23303 assertNull(calibrator.getEstimatedMa());
23304 assertNull(calibrator.getEstimatedSx());
23305 assertNull(calibrator.getEstimatedSy());
23306 assertNull(calibrator.getEstimatedSz());
23307 assertNull(calibrator.getEstimatedMxy());
23308 assertNull(calibrator.getEstimatedMxz());
23309 assertNull(calibrator.getEstimatedMyx());
23310 assertNull(calibrator.getEstimatedMyz());
23311 assertNull(calibrator.getEstimatedMzx());
23312 assertNull(calibrator.getEstimatedMzy());
23313 assertNull(calibrator.getEstimatedCovariance());
23314 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23315 assertNotNull(calibrator.getGroundTruthGravityNorm());
23316 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23317 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23318 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23319 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23320 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23321 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23322 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23323
23324
23325 final Acceleration invalidGravityNorm = new Acceleration(
23326 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23327
23328 calibrator = null;
23329 try {
23330 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23331 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz,
23332 mxy, mxz, myx, myz, mzx, mzy);
23333 fail("IllegalArgumentException expected but not thrown");
23334 } catch (final IllegalArgumentException ignore) {
23335 }
23336 assertNull(calibrator);
23337 }
23338
23339 @Test
23340 public void testConstructor197() throws WrongSizeException {
23341 final Collection<StandardDeviationBodyKinematics> measurements =
23342 Collections.emptyList();
23343
23344 final Matrix ba = generateBa();
23345 final double biasX = ba.getElementAtIndex(0);
23346 final double biasY = ba.getElementAtIndex(1);
23347 final double biasZ = ba.getElementAtIndex(2);
23348
23349 final Matrix ma = generateMaCommonAxis();
23350 final double sx = ma.getElementAt(0, 0);
23351 final double sy = ma.getElementAt(1, 1);
23352 final double sz = ma.getElementAt(2, 2);
23353 final double mxy = ma.getElementAt(0, 1);
23354 final double mxz = ma.getElementAt(0, 2);
23355 final double myx = ma.getElementAt(1, 0);
23356 final double myz = ma.getElementAt(1, 2);
23357 final double mzx = ma.getElementAt(2, 0);
23358 final double mzy = ma.getElementAt(2, 1);
23359
23360 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23361 final double latitude = Math.toRadians(
23362 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23363 final double longitude = Math.toRadians(
23364 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23365 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23366 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23367 final NEDVelocity nedVelocity = new NEDVelocity();
23368 final ECEFPosition ecefPosition = new ECEFPosition();
23369 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23370 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23371 ecefPosition, ecefVelocity);
23372 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23373 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23374 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23375
23376 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23377 new KnownBiasAndGravityNormAccelerometerCalibrator(
23378 gravityNorm, measurements,
23379 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23380 myx, myz, mzx, mzy);
23381
23382
23383 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23384 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23385 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23386 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23387 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23388 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23389 final Acceleration bx2 = new Acceleration(0.0,
23390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23391 calibrator.getBiasXAsAcceleration(bx2);
23392 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23393 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23394 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23395 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23396 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23397 final Acceleration by2 = new Acceleration(0.0,
23398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23399 calibrator.getBiasYAsAcceleration(by2);
23400 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23401 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23402 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23403 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23404 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23405 final Acceleration bz2 = new Acceleration(0.0,
23406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23407 calibrator.getBiasZAsAcceleration(bz2);
23408 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23409 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23410 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23411 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23412 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23413 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23414 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23415 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23416 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23417 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23418 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23419 final double[] bias1 = calibrator.getBias();
23420 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23421 final double[] bias2 = new double[3];
23422 calibrator.getBias(bias2);
23423 assertArrayEquals(bias1, bias2, 0.0);
23424 final Matrix b1 = calibrator.getBiasAsMatrix();
23425 assertEquals(b1, ba);
23426 final Matrix b2 = new Matrix(3, 1);
23427 calibrator.getBiasAsMatrix(b2);
23428 assertEquals(b1, b2);
23429 final Matrix ma1 = new Matrix(3, 3);
23430 ma1.setSubmatrix(0, 0,
23431 2, 2,
23432 new double[]{sx, myx, mzx,
23433 mxy, sy, mzy,
23434 mxz, myz, sz});
23435 assertEquals(calibrator.getInitialMa(), ma1);
23436 final Matrix ma2 = new Matrix(3, 3);
23437 calibrator.getInitialMa(ma2);
23438 assertEquals(ma1, ma2);
23439 assertSame(calibrator.getMeasurements(), measurements);
23440 assertFalse(calibrator.isCommonAxisUsed());
23441 assertNull(calibrator.getListener());
23442 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23443 assertFalse(calibrator.isReady());
23444 assertFalse(calibrator.isRunning());
23445 assertNull(calibrator.getEstimatedMa());
23446 assertNull(calibrator.getEstimatedSx());
23447 assertNull(calibrator.getEstimatedSy());
23448 assertNull(calibrator.getEstimatedSz());
23449 assertNull(calibrator.getEstimatedMxy());
23450 assertNull(calibrator.getEstimatedMxz());
23451 assertNull(calibrator.getEstimatedMyx());
23452 assertNull(calibrator.getEstimatedMyz());
23453 assertNull(calibrator.getEstimatedMzx());
23454 assertNull(calibrator.getEstimatedMzy());
23455 assertNull(calibrator.getEstimatedCovariance());
23456 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23457 assertNotNull(calibrator.getGroundTruthGravityNorm());
23458 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23459 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23460 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23461 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23462 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23463 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23464 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23465
23466
23467 final Acceleration invalidGravityNorm = new Acceleration(
23468 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23469
23470 calibrator = null;
23471 try {
23472 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23473 invalidGravityNorm, measurements,
23474 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23475 myx, myz, mzx, mzy);
23476 fail("IllegalArgumentException expected but not thrown");
23477 } catch (final IllegalArgumentException ignore) {
23478 }
23479 assertNull(calibrator);
23480 }
23481
23482 @Test
23483 public void testConstructor198() throws WrongSizeException {
23484 final Collection<StandardDeviationBodyKinematics> measurements =
23485 Collections.emptyList();
23486
23487 final Matrix ba = generateBa();
23488 final double biasX = ba.getElementAtIndex(0);
23489 final double biasY = ba.getElementAtIndex(1);
23490 final double biasZ = ba.getElementAtIndex(2);
23491
23492 final Matrix ma = generateMaCommonAxis();
23493 final double sx = ma.getElementAt(0, 0);
23494 final double sy = ma.getElementAt(1, 1);
23495 final double sz = ma.getElementAt(2, 2);
23496 final double mxy = ma.getElementAt(0, 1);
23497 final double mxz = ma.getElementAt(0, 2);
23498 final double myx = ma.getElementAt(1, 0);
23499 final double myz = ma.getElementAt(1, 2);
23500 final double mzx = ma.getElementAt(2, 0);
23501 final double mzy = ma.getElementAt(2, 1);
23502
23503 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23504 final double latitude = Math.toRadians(
23505 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23506 final double longitude = Math.toRadians(
23507 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23508 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23509 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23510 final NEDVelocity nedVelocity = new NEDVelocity();
23511 final ECEFPosition ecefPosition = new ECEFPosition();
23512 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23513 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23514 ecefPosition, ecefVelocity);
23515 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23516 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23517 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23518
23519 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23520 new KnownBiasAndGravityNormAccelerometerCalibrator(
23521 gravityNorm, measurements,
23522 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23523 myx, myz, mzx, mzy, this);
23524
23525
23526 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23527 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23528 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23529 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23530 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23531 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23532 final Acceleration bx2 = new Acceleration(0.0,
23533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23534 calibrator.getBiasXAsAcceleration(bx2);
23535 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23536 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23537 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23538 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23539 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23540 final Acceleration by2 = new Acceleration(0.0,
23541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23542 calibrator.getBiasYAsAcceleration(by2);
23543 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23544 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23545 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23546 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23547 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23548 final Acceleration bz2 = new Acceleration(0.0,
23549 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23550 calibrator.getBiasZAsAcceleration(bz2);
23551 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23552 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23553 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23554 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23555 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23556 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23557 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23558 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23559 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23560 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23561 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23562 final double[] bias1 = calibrator.getBias();
23563 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23564 final double[] bias2 = new double[3];
23565 calibrator.getBias(bias2);
23566 assertArrayEquals(bias1, bias2, 0.0);
23567 final Matrix b1 = calibrator.getBiasAsMatrix();
23568 assertEquals(b1, ba);
23569 final Matrix b2 = new Matrix(3, 1);
23570 calibrator.getBiasAsMatrix(b2);
23571 assertEquals(b1, b2);
23572 final Matrix ma1 = new Matrix(3, 3);
23573 ma1.setSubmatrix(0, 0,
23574 2, 2,
23575 new double[]{sx, myx, mzx,
23576 mxy, sy, mzy,
23577 mxz, myz, sz});
23578 assertEquals(calibrator.getInitialMa(), ma1);
23579 final Matrix ma2 = new Matrix(3, 3);
23580 calibrator.getInitialMa(ma2);
23581 assertEquals(ma1, ma2);
23582 assertSame(calibrator.getMeasurements(), measurements);
23583 assertFalse(calibrator.isCommonAxisUsed());
23584 assertSame(calibrator.getListener(), this);
23585 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23586 assertFalse(calibrator.isReady());
23587 assertFalse(calibrator.isRunning());
23588 assertNull(calibrator.getEstimatedMa());
23589 assertNull(calibrator.getEstimatedSx());
23590 assertNull(calibrator.getEstimatedSy());
23591 assertNull(calibrator.getEstimatedSz());
23592 assertNull(calibrator.getEstimatedMxy());
23593 assertNull(calibrator.getEstimatedMxz());
23594 assertNull(calibrator.getEstimatedMyx());
23595 assertNull(calibrator.getEstimatedMyz());
23596 assertNull(calibrator.getEstimatedMzx());
23597 assertNull(calibrator.getEstimatedMzy());
23598 assertNull(calibrator.getEstimatedCovariance());
23599 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23600 assertNotNull(calibrator.getGroundTruthGravityNorm());
23601 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23602 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23603 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23604 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23605 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23606 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23607 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23608
23609
23610 final Acceleration invalidGravityNorm = new Acceleration(
23611 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23612
23613 calibrator = null;
23614 try {
23615 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23616 invalidGravityNorm, measurements,
23617 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23618 myx, myz, mzx, mzy, this);
23619 fail("IllegalArgumentException expected but not thrown");
23620 } catch (final IllegalArgumentException ignore) {
23621 }
23622 assertNull(calibrator);
23623 }
23624
23625 @Test
23626 public void testConstructor199() throws WrongSizeException {
23627 final Matrix ba = generateBa();
23628 final double biasX = ba.getElementAtIndex(0);
23629 final double biasY = ba.getElementAtIndex(1);
23630 final double biasZ = ba.getElementAtIndex(2);
23631
23632 final Matrix ma = generateMaCommonAxis();
23633 final double sx = ma.getElementAt(0, 0);
23634 final double sy = ma.getElementAt(1, 1);
23635 final double sz = ma.getElementAt(2, 2);
23636 final double mxy = ma.getElementAt(0, 1);
23637 final double mxz = ma.getElementAt(0, 2);
23638 final double myx = ma.getElementAt(1, 0);
23639 final double myz = ma.getElementAt(1, 2);
23640 final double mzx = ma.getElementAt(2, 0);
23641 final double mzy = ma.getElementAt(2, 1);
23642
23643 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23644 final double latitude = Math.toRadians(
23645 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23646 final double longitude = Math.toRadians(
23647 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23648 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23649 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23650 final NEDVelocity nedVelocity = new NEDVelocity();
23651 final ECEFPosition ecefPosition = new ECEFPosition();
23652 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23653 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23654 ecefPosition, ecefVelocity);
23655 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23656 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23657 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23658
23659 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23660 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23661 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23662 myx, myz, mzx, mzy);
23663
23664
23665 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23666 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23667 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23668 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23669 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23670 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23671 final Acceleration bx2 = new Acceleration(0.0,
23672 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23673 calibrator.getBiasXAsAcceleration(bx2);
23674 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23675 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23676 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23677 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23678 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23679 final Acceleration by2 = new Acceleration(0.0,
23680 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23681 calibrator.getBiasYAsAcceleration(by2);
23682 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23683 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23684 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23685 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23686 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23687 final Acceleration bz2 = new Acceleration(0.0,
23688 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23689 calibrator.getBiasZAsAcceleration(bz2);
23690 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23691 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23692 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23693 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23694 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23695 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23696 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23697 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23698 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23699 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23700 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23701 final double[] bias1 = calibrator.getBias();
23702 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23703 final double[] bias2 = new double[3];
23704 calibrator.getBias(bias2);
23705 assertArrayEquals(bias1, bias2, 0.0);
23706 final Matrix b1 = calibrator.getBiasAsMatrix();
23707 assertEquals(b1, ba);
23708 final Matrix b2 = new Matrix(3, 1);
23709 calibrator.getBiasAsMatrix(b2);
23710 assertEquals(b1, b2);
23711 final Matrix ma1 = new Matrix(3, 3);
23712 ma1.setSubmatrix(0, 0,
23713 2, 2,
23714 new double[]{sx, myx, mzx,
23715 mxy, sy, mzy,
23716 mxz, myz, sz});
23717 assertEquals(calibrator.getInitialMa(), ma1);
23718 final Matrix ma2 = new Matrix(3, 3);
23719 calibrator.getInitialMa(ma2);
23720 assertEquals(ma1, ma2);
23721 assertNull(calibrator.getMeasurements());
23722 assertTrue(calibrator.isCommonAxisUsed());
23723 assertNull(calibrator.getListener());
23724 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23725 assertFalse(calibrator.isReady());
23726 assertFalse(calibrator.isRunning());
23727 assertNull(calibrator.getEstimatedMa());
23728 assertNull(calibrator.getEstimatedSx());
23729 assertNull(calibrator.getEstimatedSy());
23730 assertNull(calibrator.getEstimatedSz());
23731 assertNull(calibrator.getEstimatedMxy());
23732 assertNull(calibrator.getEstimatedMxz());
23733 assertNull(calibrator.getEstimatedMyx());
23734 assertNull(calibrator.getEstimatedMyz());
23735 assertNull(calibrator.getEstimatedMzx());
23736 assertNull(calibrator.getEstimatedMzy());
23737 assertNull(calibrator.getEstimatedCovariance());
23738 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23739 assertNotNull(calibrator.getGroundTruthGravityNorm());
23740 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23741 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23742 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23743 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23744 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23745 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23746 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23747
23748
23749 final Acceleration invalidGravityNorm = new Acceleration(
23750 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23751
23752 calibrator = null;
23753 try {
23754 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23755 invalidGravityNorm, true, biasX, biasY, biasZ,
23756 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23757 fail("IllegalArgumentException expected but not thrown");
23758 } catch (final IllegalArgumentException ignore) {
23759 }
23760 assertNull(calibrator);
23761 }
23762
23763 @Test
23764 public void testConstructor200() throws WrongSizeException {
23765 final Matrix ba = generateBa();
23766 final double biasX = ba.getElementAtIndex(0);
23767 final double biasY = ba.getElementAtIndex(1);
23768 final double biasZ = ba.getElementAtIndex(2);
23769
23770 final Matrix ma = generateMaCommonAxis();
23771 final double sx = ma.getElementAt(0, 0);
23772 final double sy = ma.getElementAt(1, 1);
23773 final double sz = ma.getElementAt(2, 2);
23774 final double mxy = ma.getElementAt(0, 1);
23775 final double mxz = ma.getElementAt(0, 2);
23776 final double myx = ma.getElementAt(1, 0);
23777 final double myz = ma.getElementAt(1, 2);
23778 final double mzx = ma.getElementAt(2, 0);
23779 final double mzy = ma.getElementAt(2, 1);
23780
23781 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23782 final double latitude = Math.toRadians(
23783 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23784 final double longitude = Math.toRadians(
23785 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23786 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23787 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23788 final NEDVelocity nedVelocity = new NEDVelocity();
23789 final ECEFPosition ecefPosition = new ECEFPosition();
23790 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23791 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23792 ecefPosition, ecefVelocity);
23793 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23794 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23795 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23796
23797 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23798 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23799 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23800 myx, myz, mzx, mzy, this);
23801
23802
23803 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23804 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23805 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23806 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23807 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23808 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23809 final Acceleration bx2 = new Acceleration(0.0,
23810 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23811 calibrator.getBiasXAsAcceleration(bx2);
23812 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23813 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23814 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23815 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23816 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23817 final Acceleration by2 = new Acceleration(0.0,
23818 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23819 calibrator.getBiasYAsAcceleration(by2);
23820 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23821 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23822 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23823 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23824 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23825 final Acceleration bz2 = new Acceleration(0.0,
23826 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23827 calibrator.getBiasZAsAcceleration(bz2);
23828 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23829 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23830 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23831 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23832 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23833 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23834 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23835 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23836 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23837 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23838 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23839 final double[] bias1 = calibrator.getBias();
23840 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23841 final double[] bias2 = new double[3];
23842 calibrator.getBias(bias2);
23843 assertArrayEquals(bias1, bias2, 0.0);
23844 final Matrix b1 = calibrator.getBiasAsMatrix();
23845 assertEquals(b1, ba);
23846 final Matrix b2 = new Matrix(3, 1);
23847 calibrator.getBiasAsMatrix(b2);
23848 assertEquals(b1, b2);
23849 final Matrix ma1 = new Matrix(3, 3);
23850 ma1.setSubmatrix(0, 0,
23851 2, 2,
23852 new double[]{sx, myx, mzx,
23853 mxy, sy, mzy,
23854 mxz, myz, sz});
23855 assertEquals(calibrator.getInitialMa(), ma1);
23856 final Matrix ma2 = new Matrix(3, 3);
23857 calibrator.getInitialMa(ma2);
23858 assertEquals(ma1, ma2);
23859 assertNull(calibrator.getMeasurements());
23860 assertTrue(calibrator.isCommonAxisUsed());
23861 assertSame(calibrator.getListener(), this);
23862 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23863 assertFalse(calibrator.isReady());
23864 assertFalse(calibrator.isRunning());
23865 assertNull(calibrator.getEstimatedMa());
23866 assertNull(calibrator.getEstimatedSx());
23867 assertNull(calibrator.getEstimatedSy());
23868 assertNull(calibrator.getEstimatedSz());
23869 assertNull(calibrator.getEstimatedMxy());
23870 assertNull(calibrator.getEstimatedMxz());
23871 assertNull(calibrator.getEstimatedMyx());
23872 assertNull(calibrator.getEstimatedMyz());
23873 assertNull(calibrator.getEstimatedMzx());
23874 assertNull(calibrator.getEstimatedMzy());
23875 assertNull(calibrator.getEstimatedCovariance());
23876 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23877 assertNotNull(calibrator.getGroundTruthGravityNorm());
23878 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23879 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23880 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23881 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23882 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23883 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23884 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23885
23886
23887 final Acceleration invalidGravityNorm = new Acceleration(
23888 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23889
23890 calibrator = null;
23891 try {
23892 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23893 invalidGravityNorm, true, biasX, biasY, biasZ,
23894 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
23895 this);
23896 fail("IllegalArgumentException expected but not thrown");
23897 } catch (final IllegalArgumentException ignore) {
23898 }
23899 assertNull(calibrator);
23900 }
23901
23902 @Test
23903 public void testConstructor201() throws WrongSizeException {
23904 final Collection<StandardDeviationBodyKinematics> measurements =
23905 Collections.emptyList();
23906
23907 final Matrix ba = generateBa();
23908 final double biasX = ba.getElementAtIndex(0);
23909 final double biasY = ba.getElementAtIndex(1);
23910 final double biasZ = ba.getElementAtIndex(2);
23911
23912 final Matrix ma = generateMaCommonAxis();
23913 final double sx = ma.getElementAt(0, 0);
23914 final double sy = ma.getElementAt(1, 1);
23915 final double sz = ma.getElementAt(2, 2);
23916 final double mxy = ma.getElementAt(0, 1);
23917 final double mxz = ma.getElementAt(0, 2);
23918 final double myx = ma.getElementAt(1, 0);
23919 final double myz = ma.getElementAt(1, 2);
23920 final double mzx = ma.getElementAt(2, 0);
23921 final double mzy = ma.getElementAt(2, 1);
23922
23923 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23924 final double latitude = Math.toRadians(
23925 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23926 final double longitude = Math.toRadians(
23927 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23928 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23929 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23930 final NEDVelocity nedVelocity = new NEDVelocity();
23931 final ECEFPosition ecefPosition = new ECEFPosition();
23932 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23933 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23934 ecefPosition, ecefVelocity);
23935 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23936 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23937 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23938
23939 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23940 new KnownBiasAndGravityNormAccelerometerCalibrator(
23941 gravityNorm, measurements,
23942 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23943 myx, myz, mzx, mzy);
23944
23945
23946 assertEquals(calibrator.getBiasX(), biasX, 0.0);
23947 assertEquals(calibrator.getBiasY(), biasY, 0.0);
23948 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23949 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23950 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23951 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23952 final Acceleration bx2 = new Acceleration(0.0,
23953 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23954 calibrator.getBiasXAsAcceleration(bx2);
23955 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23956 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23957 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23958 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23959 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960 final Acceleration by2 = new Acceleration(0.0,
23961 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23962 calibrator.getBiasYAsAcceleration(by2);
23963 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23964 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23965 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23966 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23967 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23968 final Acceleration bz2 = new Acceleration(0.0,
23969 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23970 calibrator.getBiasZAsAcceleration(bz2);
23971 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23972 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23973 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23974 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23975 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23976 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23977 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23978 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23979 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23980 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23981 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23982 final double[] bias1 = calibrator.getBias();
23983 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23984 final double[] bias2 = new double[3];
23985 calibrator.getBias(bias2);
23986 assertArrayEquals(bias1, bias2, 0.0);
23987 final Matrix b1 = calibrator.getBiasAsMatrix();
23988 assertEquals(b1, ba);
23989 final Matrix b2 = new Matrix(3, 1);
23990 calibrator.getBiasAsMatrix(b2);
23991 assertEquals(b1, b2);
23992 final Matrix ma1 = new Matrix(3, 3);
23993 ma1.setSubmatrix(0, 0,
23994 2, 2,
23995 new double[]{sx, myx, mzx,
23996 mxy, sy, mzy,
23997 mxz, myz, sz});
23998 assertEquals(calibrator.getInitialMa(), ma1);
23999 final Matrix ma2 = new Matrix(3, 3);
24000 calibrator.getInitialMa(ma2);
24001 assertEquals(ma1, ma2);
24002 assertSame(calibrator.getMeasurements(), measurements);
24003 assertTrue(calibrator.isCommonAxisUsed());
24004 assertNull(calibrator.getListener());
24005 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24006 assertFalse(calibrator.isReady());
24007 assertFalse(calibrator.isRunning());
24008 assertNull(calibrator.getEstimatedMa());
24009 assertNull(calibrator.getEstimatedSx());
24010 assertNull(calibrator.getEstimatedSy());
24011 assertNull(calibrator.getEstimatedSz());
24012 assertNull(calibrator.getEstimatedMxy());
24013 assertNull(calibrator.getEstimatedMxz());
24014 assertNull(calibrator.getEstimatedMyx());
24015 assertNull(calibrator.getEstimatedMyz());
24016 assertNull(calibrator.getEstimatedMzx());
24017 assertNull(calibrator.getEstimatedMzy());
24018 assertNull(calibrator.getEstimatedCovariance());
24019 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24020 assertNotNull(calibrator.getGroundTruthGravityNorm());
24021 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24022 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24023 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24024 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24025 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24026 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24027 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24028
24029
24030 final Acceleration invalidGravityNorm = new Acceleration(
24031 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24032
24033 calibrator = null;
24034 try {
24035 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24036 invalidGravityNorm, measurements,
24037 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24038 myx, myz, mzx, mzy);
24039 fail("IllegalArgumentException expected but not thrown");
24040 } catch (final IllegalArgumentException ignore) {
24041 }
24042 assertNull(calibrator);
24043 }
24044
24045 @Test
24046 public void testConstructor202() throws WrongSizeException {
24047 final Collection<StandardDeviationBodyKinematics> measurements =
24048 Collections.emptyList();
24049
24050 final Matrix ba = generateBa();
24051 final double biasX = ba.getElementAtIndex(0);
24052 final double biasY = ba.getElementAtIndex(1);
24053 final double biasZ = ba.getElementAtIndex(2);
24054
24055 final Matrix ma = generateMaCommonAxis();
24056 final double sx = ma.getElementAt(0, 0);
24057 final double sy = ma.getElementAt(1, 1);
24058 final double sz = ma.getElementAt(2, 2);
24059 final double mxy = ma.getElementAt(0, 1);
24060 final double mxz = ma.getElementAt(0, 2);
24061 final double myx = ma.getElementAt(1, 0);
24062 final double myz = ma.getElementAt(1, 2);
24063 final double mzx = ma.getElementAt(2, 0);
24064 final double mzy = ma.getElementAt(2, 1);
24065
24066 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24067 final double latitude = Math.toRadians(
24068 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24069 final double longitude = Math.toRadians(
24070 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24071 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24072 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24073 final NEDVelocity nedVelocity = new NEDVelocity();
24074 final ECEFPosition ecefPosition = new ECEFPosition();
24075 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24076 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24077 ecefPosition, ecefVelocity);
24078 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24079 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24080 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24081
24082 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24083 new KnownBiasAndGravityNormAccelerometerCalibrator(
24084 gravityNorm, measurements,
24085 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24086 myx, myz, mzx, mzy, this);
24087
24088
24089 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24090 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24091 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24092 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24093 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24094 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24095 final Acceleration bx2 = new Acceleration(0.0,
24096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24097 calibrator.getBiasXAsAcceleration(bx2);
24098 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24099 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24100 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24101 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24102 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24103 final Acceleration by2 = new Acceleration(0.0,
24104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24105 calibrator.getBiasYAsAcceleration(by2);
24106 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24107 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24108 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24109 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24110 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24111 final Acceleration bz2 = new Acceleration(0.0,
24112 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24113 calibrator.getBiasZAsAcceleration(bz2);
24114 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24115 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24116 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24117 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24118 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24119 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24120 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24121 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24122 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24123 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24124 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24125 final double[] bias1 = calibrator.getBias();
24126 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24127 final double[] bias2 = new double[3];
24128 calibrator.getBias(bias2);
24129 assertArrayEquals(bias1, bias2, 0.0);
24130 final Matrix b1 = calibrator.getBiasAsMatrix();
24131 assertEquals(b1, ba);
24132 final Matrix b2 = new Matrix(3, 1);
24133 calibrator.getBiasAsMatrix(b2);
24134 assertEquals(b1, b2);
24135 final Matrix ma1 = new Matrix(3, 3);
24136 ma1.setSubmatrix(0, 0,
24137 2, 2,
24138 new double[]{sx, myx, mzx,
24139 mxy, sy, mzy,
24140 mxz, myz, sz});
24141 assertEquals(calibrator.getInitialMa(), ma1);
24142 final Matrix ma2 = new Matrix(3, 3);
24143 calibrator.getInitialMa(ma2);
24144 assertEquals(ma1, ma2);
24145 assertSame(calibrator.getMeasurements(), measurements);
24146 assertTrue(calibrator.isCommonAxisUsed());
24147 assertSame(calibrator.getListener(), this);
24148 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24149 assertFalse(calibrator.isReady());
24150 assertFalse(calibrator.isRunning());
24151 assertNull(calibrator.getEstimatedMa());
24152 assertNull(calibrator.getEstimatedSx());
24153 assertNull(calibrator.getEstimatedSy());
24154 assertNull(calibrator.getEstimatedSz());
24155 assertNull(calibrator.getEstimatedMxy());
24156 assertNull(calibrator.getEstimatedMxz());
24157 assertNull(calibrator.getEstimatedMyx());
24158 assertNull(calibrator.getEstimatedMyz());
24159 assertNull(calibrator.getEstimatedMzx());
24160 assertNull(calibrator.getEstimatedMzy());
24161 assertNull(calibrator.getEstimatedCovariance());
24162 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24163 assertNotNull(calibrator.getGroundTruthGravityNorm());
24164 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24165 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24166 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24167 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24168 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24169 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24170 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24171
24172
24173 final Acceleration invalidGravityNorm = new Acceleration(
24174 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24175
24176 calibrator = null;
24177 try {
24178 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24179 invalidGravityNorm, measurements,
24180 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24181 myx, myz, mzx, mzy, this);
24182 fail("IllegalArgumentException expected but not thrown");
24183 } catch (final IllegalArgumentException ignore) {
24184 }
24185 assertNull(calibrator);
24186 }
24187
24188 @Test
24189 public void testConstructor203() throws WrongSizeException {
24190 final Matrix ba = generateBa();
24191 final double biasX = ba.getElementAtIndex(0);
24192 final double biasY = ba.getElementAtIndex(1);
24193 final double biasZ = ba.getElementAtIndex(2);
24194
24195 final Acceleration bx = new Acceleration(biasX,
24196 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24197 final Acceleration by = new Acceleration(biasY,
24198 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24199 final Acceleration bz = new Acceleration(biasZ,
24200 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24201
24202 final Matrix ma = generateMaCommonAxis();
24203 final double sx = ma.getElementAt(0, 0);
24204 final double sy = ma.getElementAt(1, 1);
24205 final double sz = ma.getElementAt(2, 2);
24206 final double mxy = ma.getElementAt(0, 1);
24207 final double mxz = ma.getElementAt(0, 2);
24208 final double myx = ma.getElementAt(1, 0);
24209 final double myz = ma.getElementAt(1, 2);
24210 final double mzx = ma.getElementAt(2, 0);
24211 final double mzy = ma.getElementAt(2, 1);
24212
24213 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24214 final double latitude = Math.toRadians(
24215 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24216 final double longitude = Math.toRadians(
24217 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24218 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24219 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24220 final NEDVelocity nedVelocity = new NEDVelocity();
24221 final ECEFPosition ecefPosition = new ECEFPosition();
24222 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24223 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24224 ecefPosition, ecefVelocity);
24225 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24226 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24227 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24228
24229 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24230 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24231 bx, by, bz, sx, sy, sz, mxy, mxz,
24232 myx, myz, mzx, mzy);
24233
24234
24235 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24236 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24237 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24238 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24239 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24240 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24241 final Acceleration bx2 = new Acceleration(0.0,
24242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24243 calibrator.getBiasXAsAcceleration(bx2);
24244 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24245 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24246 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24247 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24248 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24249 final Acceleration by2 = new Acceleration(0.0,
24250 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24251 calibrator.getBiasYAsAcceleration(by2);
24252 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24253 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24254 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24255 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24256 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24257 final Acceleration bz2 = new Acceleration(0.0,
24258 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24259 calibrator.getBiasZAsAcceleration(bz2);
24260 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24261 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24262 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24263 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24264 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24265 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24266 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24267 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24268 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24269 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24270 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24271 final double[] bias1 = calibrator.getBias();
24272 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24273 final double[] bias2 = new double[3];
24274 calibrator.getBias(bias2);
24275 assertArrayEquals(bias1, bias2, 0.0);
24276 final Matrix b1 = calibrator.getBiasAsMatrix();
24277 assertEquals(b1, ba);
24278 final Matrix b2 = new Matrix(3, 1);
24279 calibrator.getBiasAsMatrix(b2);
24280 assertEquals(b1, b2);
24281 final Matrix ma1 = new Matrix(3, 3);
24282 ma1.setSubmatrix(0, 0,
24283 2, 2,
24284 new double[]{sx, myx, mzx,
24285 mxy, sy, mzy,
24286 mxz, myz, sz});
24287 assertEquals(calibrator.getInitialMa(), ma1);
24288 final Matrix ma2 = new Matrix(3, 3);
24289 calibrator.getInitialMa(ma2);
24290 assertEquals(ma1, ma2);
24291 assertNull(calibrator.getMeasurements());
24292 assertFalse(calibrator.isCommonAxisUsed());
24293 assertNull(calibrator.getListener());
24294 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24295 assertFalse(calibrator.isReady());
24296 assertFalse(calibrator.isRunning());
24297 assertNull(calibrator.getEstimatedMa());
24298 assertNull(calibrator.getEstimatedSx());
24299 assertNull(calibrator.getEstimatedSy());
24300 assertNull(calibrator.getEstimatedSz());
24301 assertNull(calibrator.getEstimatedMxy());
24302 assertNull(calibrator.getEstimatedMxz());
24303 assertNull(calibrator.getEstimatedMyx());
24304 assertNull(calibrator.getEstimatedMyz());
24305 assertNull(calibrator.getEstimatedMzx());
24306 assertNull(calibrator.getEstimatedMzy());
24307 assertNull(calibrator.getEstimatedCovariance());
24308 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24309 assertNotNull(calibrator.getGroundTruthGravityNorm());
24310 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24311 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24312 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24313 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24314 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24315 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24316 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24317
24318
24319 final Acceleration invalidGravityNorm = new Acceleration(
24320 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24321
24322 calibrator = null;
24323 try {
24324 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24325 invalidGravityNorm, bx, by, bz, sx, sy, sz,
24326 mxy, mxz, myx, myz, mzx, mzy);
24327 fail("IllegalArgumentException expected but not thrown");
24328 } catch (final IllegalArgumentException ignore) {
24329 }
24330 assertNull(calibrator);
24331 }
24332
24333 @Test
24334 public void testConstructor204() throws WrongSizeException {
24335 final Matrix ba = generateBa();
24336 final double biasX = ba.getElementAtIndex(0);
24337 final double biasY = ba.getElementAtIndex(1);
24338 final double biasZ = ba.getElementAtIndex(2);
24339
24340 final Acceleration bx = new Acceleration(biasX,
24341 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24342 final Acceleration by = new Acceleration(biasY,
24343 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24344 final Acceleration bz = new Acceleration(biasZ,
24345 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24346
24347 final Matrix ma = generateMaCommonAxis();
24348 final double sx = ma.getElementAt(0, 0);
24349 final double sy = ma.getElementAt(1, 1);
24350 final double sz = ma.getElementAt(2, 2);
24351 final double mxy = ma.getElementAt(0, 1);
24352 final double mxz = ma.getElementAt(0, 2);
24353 final double myx = ma.getElementAt(1, 0);
24354 final double myz = ma.getElementAt(1, 2);
24355 final double mzx = ma.getElementAt(2, 0);
24356 final double mzy = ma.getElementAt(2, 1);
24357
24358 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24359 final double latitude = Math.toRadians(
24360 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24361 final double longitude = Math.toRadians(
24362 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24363 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24364 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24365 final NEDVelocity nedVelocity = new NEDVelocity();
24366 final ECEFPosition ecefPosition = new ECEFPosition();
24367 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24368 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24369 ecefPosition, ecefVelocity);
24370 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24371 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24372 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24373
24374 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24375 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24376 bx, by, bz, sx, sy, sz, mxy, mxz,
24377 myx, myz, mzx, mzy, this);
24378
24379
24380 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24381 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24382 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24383 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24384 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24385 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24386 final Acceleration bx2 = new Acceleration(0.0,
24387 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24388 calibrator.getBiasXAsAcceleration(bx2);
24389 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24390 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24391 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24392 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24393 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24394 final Acceleration by2 = new Acceleration(0.0,
24395 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24396 calibrator.getBiasYAsAcceleration(by2);
24397 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24398 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24399 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24400 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24401 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24402 final Acceleration bz2 = new Acceleration(0.0,
24403 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24404 calibrator.getBiasZAsAcceleration(bz2);
24405 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24406 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24407 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24408 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24409 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24410 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24411 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24412 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24413 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24414 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24415 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24416 final double[] bias1 = calibrator.getBias();
24417 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24418 final double[] bias2 = new double[3];
24419 calibrator.getBias(bias2);
24420 assertArrayEquals(bias1, bias2, 0.0);
24421 final Matrix b1 = calibrator.getBiasAsMatrix();
24422 assertEquals(b1, ba);
24423 final Matrix b2 = new Matrix(3, 1);
24424 calibrator.getBiasAsMatrix(b2);
24425 assertEquals(b1, b2);
24426 final Matrix ma1 = new Matrix(3, 3);
24427 ma1.setSubmatrix(0, 0,
24428 2, 2,
24429 new double[]{sx, myx, mzx,
24430 mxy, sy, mzy,
24431 mxz, myz, sz});
24432 assertEquals(calibrator.getInitialMa(), ma1);
24433 final Matrix ma2 = new Matrix(3, 3);
24434 calibrator.getInitialMa(ma2);
24435 assertEquals(ma1, ma2);
24436 assertNull(calibrator.getMeasurements());
24437 assertFalse(calibrator.isCommonAxisUsed());
24438 assertSame(calibrator.getListener(), this);
24439 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24440 assertFalse(calibrator.isReady());
24441 assertFalse(calibrator.isRunning());
24442 assertNull(calibrator.getEstimatedMa());
24443 assertNull(calibrator.getEstimatedSx());
24444 assertNull(calibrator.getEstimatedSy());
24445 assertNull(calibrator.getEstimatedSz());
24446 assertNull(calibrator.getEstimatedMxy());
24447 assertNull(calibrator.getEstimatedMxz());
24448 assertNull(calibrator.getEstimatedMyx());
24449 assertNull(calibrator.getEstimatedMyz());
24450 assertNull(calibrator.getEstimatedMzx());
24451 assertNull(calibrator.getEstimatedMzy());
24452 assertNull(calibrator.getEstimatedCovariance());
24453 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24454 assertNotNull(calibrator.getGroundTruthGravityNorm());
24455 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24456 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24457 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24458 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24459 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24460 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24461 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24462
24463
24464 final Acceleration invalidGravityNorm = new Acceleration(
24465 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24466
24467 calibrator = null;
24468 try {
24469 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24470 invalidGravityNorm, bx, by, bz, sx, sy, sz,
24471 mxy, mxz, myx, myz, mzx, mzy, this);
24472 fail("IllegalArgumentException expected but not thrown");
24473 } catch (final IllegalArgumentException ignore) {
24474 }
24475 assertNull(calibrator);
24476 }
24477
24478 @Test
24479 public void testConstructor205() throws WrongSizeException {
24480 final Collection<StandardDeviationBodyKinematics> measurements =
24481 Collections.emptyList();
24482
24483 final Matrix ba = generateBa();
24484 final double biasX = ba.getElementAtIndex(0);
24485 final double biasY = ba.getElementAtIndex(1);
24486 final double biasZ = ba.getElementAtIndex(2);
24487
24488 final Acceleration bx = new Acceleration(biasX,
24489 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24490 final Acceleration by = new Acceleration(biasY,
24491 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24492 final Acceleration bz = new Acceleration(biasZ,
24493 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24494
24495 final Matrix ma = generateMaCommonAxis();
24496 final double sx = ma.getElementAt(0, 0);
24497 final double sy = ma.getElementAt(1, 1);
24498 final double sz = ma.getElementAt(2, 2);
24499 final double mxy = ma.getElementAt(0, 1);
24500 final double mxz = ma.getElementAt(0, 2);
24501 final double myx = ma.getElementAt(1, 0);
24502 final double myz = ma.getElementAt(1, 2);
24503 final double mzx = ma.getElementAt(2, 0);
24504 final double mzy = ma.getElementAt(2, 1);
24505
24506 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24507 final double latitude = Math.toRadians(
24508 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24509 final double longitude = Math.toRadians(
24510 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24511 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24512 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24513 final NEDVelocity nedVelocity = new NEDVelocity();
24514 final ECEFPosition ecefPosition = new ECEFPosition();
24515 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24516 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24517 ecefPosition, ecefVelocity);
24518 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24519 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24520 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24521
24522 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24523 new KnownBiasAndGravityNormAccelerometerCalibrator(
24524 gravityNorm, measurements,
24525 bx, by, bz, sx, sy, sz, mxy, mxz,
24526 myx, myz, mzx, mzy);
24527
24528
24529 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24530 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24531 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24532 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24533 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24534 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24535 final Acceleration bx2 = new Acceleration(0.0,
24536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24537 calibrator.getBiasXAsAcceleration(bx2);
24538 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24539 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24540 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24541 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24542 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24543 final Acceleration by2 = new Acceleration(0.0,
24544 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24545 calibrator.getBiasYAsAcceleration(by2);
24546 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24547 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24548 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24549 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24550 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24551 final Acceleration bz2 = new Acceleration(0.0,
24552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24553 calibrator.getBiasZAsAcceleration(bz2);
24554 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24555 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24556 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24557 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24558 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24559 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24560 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24561 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24562 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24563 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24564 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24565 final double[] bias1 = calibrator.getBias();
24566 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24567 final double[] bias2 = new double[3];
24568 calibrator.getBias(bias2);
24569 assertArrayEquals(bias1, bias2, 0.0);
24570 final Matrix b1 = calibrator.getBiasAsMatrix();
24571 assertEquals(b1, ba);
24572 final Matrix b2 = new Matrix(3, 1);
24573 calibrator.getBiasAsMatrix(b2);
24574 assertEquals(b1, b2);
24575 final Matrix ma1 = new Matrix(3, 3);
24576 ma1.setSubmatrix(0, 0,
24577 2, 2,
24578 new double[]{sx, myx, mzx,
24579 mxy, sy, mzy,
24580 mxz, myz, sz});
24581 assertEquals(calibrator.getInitialMa(), ma1);
24582 final Matrix ma2 = new Matrix(3, 3);
24583 calibrator.getInitialMa(ma2);
24584 assertEquals(ma1, ma2);
24585 assertSame(calibrator.getMeasurements(), measurements);
24586 assertFalse(calibrator.isCommonAxisUsed());
24587 assertNull(calibrator.getListener());
24588 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24589 assertFalse(calibrator.isReady());
24590 assertFalse(calibrator.isRunning());
24591 assertNull(calibrator.getEstimatedMa());
24592 assertNull(calibrator.getEstimatedSx());
24593 assertNull(calibrator.getEstimatedSy());
24594 assertNull(calibrator.getEstimatedSz());
24595 assertNull(calibrator.getEstimatedMxy());
24596 assertNull(calibrator.getEstimatedMxz());
24597 assertNull(calibrator.getEstimatedMyx());
24598 assertNull(calibrator.getEstimatedMyz());
24599 assertNull(calibrator.getEstimatedMzx());
24600 assertNull(calibrator.getEstimatedMzy());
24601 assertNull(calibrator.getEstimatedCovariance());
24602 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24603 assertNotNull(calibrator.getGroundTruthGravityNorm());
24604 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24605 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24606 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24607 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24608 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24609 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24610 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24611
24612
24613 final Acceleration invalidGravityNorm = new Acceleration(
24614 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24615
24616 calibrator = null;
24617 try {
24618 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24619 invalidGravityNorm, measurements,
24620 bx, by, bz, sx, sy, sz, mxy, mxz,
24621 myx, myz, mzx, mzy);
24622 fail("IllegalArgumentException expected but not thrown");
24623 } catch (final IllegalArgumentException ignore) {
24624 }
24625 assertNull(calibrator);
24626 }
24627
24628 @Test
24629 public void testConstructor206() throws WrongSizeException {
24630 final Collection<StandardDeviationBodyKinematics> measurements =
24631 Collections.emptyList();
24632
24633 final Matrix ba = generateBa();
24634 final double biasX = ba.getElementAtIndex(0);
24635 final double biasY = ba.getElementAtIndex(1);
24636 final double biasZ = ba.getElementAtIndex(2);
24637
24638 final Acceleration bx = new Acceleration(biasX,
24639 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24640 final Acceleration by = new Acceleration(biasY,
24641 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24642 final Acceleration bz = new Acceleration(biasZ,
24643 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24644
24645 final Matrix ma = generateMaCommonAxis();
24646 final double sx = ma.getElementAt(0, 0);
24647 final double sy = ma.getElementAt(1, 1);
24648 final double sz = ma.getElementAt(2, 2);
24649 final double mxy = ma.getElementAt(0, 1);
24650 final double mxz = ma.getElementAt(0, 2);
24651 final double myx = ma.getElementAt(1, 0);
24652 final double myz = ma.getElementAt(1, 2);
24653 final double mzx = ma.getElementAt(2, 0);
24654 final double mzy = ma.getElementAt(2, 1);
24655
24656 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24657 final double latitude = Math.toRadians(
24658 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24659 final double longitude = Math.toRadians(
24660 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24661 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24662 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24663 final NEDVelocity nedVelocity = new NEDVelocity();
24664 final ECEFPosition ecefPosition = new ECEFPosition();
24665 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24666 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24667 ecefPosition, ecefVelocity);
24668 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24669 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24670 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24671
24672 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24673 new KnownBiasAndGravityNormAccelerometerCalibrator(
24674 gravityNorm, measurements,
24675 bx, by, bz, sx, sy, sz, mxy, mxz,
24676 myx, myz, mzx, mzy, this);
24677
24678
24679 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24680 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24681 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24682 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24683 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24684 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24685 final Acceleration bx2 = new Acceleration(0.0,
24686 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24687 calibrator.getBiasXAsAcceleration(bx2);
24688 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24689 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24690 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24691 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24692 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693 final Acceleration by2 = new Acceleration(0.0,
24694 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24695 calibrator.getBiasYAsAcceleration(by2);
24696 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24697 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24698 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24699 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24700 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24701 final Acceleration bz2 = new Acceleration(0.0,
24702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24703 calibrator.getBiasZAsAcceleration(bz2);
24704 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24705 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24706 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24707 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24708 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24709 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24710 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24711 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24712 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24713 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24714 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24715 final double[] bias1 = calibrator.getBias();
24716 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24717 final double[] bias2 = new double[3];
24718 calibrator.getBias(bias2);
24719 assertArrayEquals(bias1, bias2, 0.0);
24720 final Matrix b1 = calibrator.getBiasAsMatrix();
24721 assertEquals(b1, ba);
24722 final Matrix b2 = new Matrix(3, 1);
24723 calibrator.getBiasAsMatrix(b2);
24724 assertEquals(b1, b2);
24725 final Matrix ma1 = new Matrix(3, 3);
24726 ma1.setSubmatrix(0, 0,
24727 2, 2,
24728 new double[]{sx, myx, mzx,
24729 mxy, sy, mzy,
24730 mxz, myz, sz});
24731 assertEquals(calibrator.getInitialMa(), ma1);
24732 final Matrix ma2 = new Matrix(3, 3);
24733 calibrator.getInitialMa(ma2);
24734 assertEquals(ma1, ma2);
24735 assertSame(calibrator.getMeasurements(), measurements);
24736 assertFalse(calibrator.isCommonAxisUsed());
24737 assertSame(calibrator.getListener(), this);
24738 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24739 assertFalse(calibrator.isReady());
24740 assertFalse(calibrator.isRunning());
24741 assertNull(calibrator.getEstimatedMa());
24742 assertNull(calibrator.getEstimatedSx());
24743 assertNull(calibrator.getEstimatedSy());
24744 assertNull(calibrator.getEstimatedSz());
24745 assertNull(calibrator.getEstimatedMxy());
24746 assertNull(calibrator.getEstimatedMxz());
24747 assertNull(calibrator.getEstimatedMyx());
24748 assertNull(calibrator.getEstimatedMyz());
24749 assertNull(calibrator.getEstimatedMzx());
24750 assertNull(calibrator.getEstimatedMzy());
24751 assertNull(calibrator.getEstimatedCovariance());
24752 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24753 assertNotNull(calibrator.getGroundTruthGravityNorm());
24754 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24755 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24756 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24757 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24758 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24759 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24760 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24761
24762
24763 final Acceleration invalidGravityNorm = new Acceleration(
24764 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24765
24766 calibrator = null;
24767 try {
24768 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24769 invalidGravityNorm, measurements,
24770 bx, by, bz, sx, sy, sz, mxy, mxz,
24771 myx, myz, mzx, mzy, this);
24772 fail("IllegalArgumentException expected but not thrown");
24773 } catch (final IllegalArgumentException ignore) {
24774 }
24775 assertNull(calibrator);
24776 }
24777
24778 @Test
24779 public void testConstructor207() throws WrongSizeException {
24780 final Matrix ba = generateBa();
24781 final double biasX = ba.getElementAtIndex(0);
24782 final double biasY = ba.getElementAtIndex(1);
24783 final double biasZ = ba.getElementAtIndex(2);
24784
24785 final Acceleration bx = new Acceleration(biasX,
24786 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24787 final Acceleration by = new Acceleration(biasY,
24788 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24789 final Acceleration bz = new Acceleration(biasZ,
24790 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24791
24792 final Matrix ma = generateMaCommonAxis();
24793 final double sx = ma.getElementAt(0, 0);
24794 final double sy = ma.getElementAt(1, 1);
24795 final double sz = ma.getElementAt(2, 2);
24796 final double mxy = ma.getElementAt(0, 1);
24797 final double mxz = ma.getElementAt(0, 2);
24798 final double myx = ma.getElementAt(1, 0);
24799 final double myz = ma.getElementAt(1, 2);
24800 final double mzx = ma.getElementAt(2, 0);
24801 final double mzy = ma.getElementAt(2, 1);
24802
24803 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24804 final double latitude = Math.toRadians(
24805 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24806 final double longitude = Math.toRadians(
24807 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24808 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24809 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24810 final NEDVelocity nedVelocity = new NEDVelocity();
24811 final ECEFPosition ecefPosition = new ECEFPosition();
24812 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24813 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24814 ecefPosition, ecefVelocity);
24815 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24816 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24817 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24818
24819 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24820 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24821 true, bx, by, bz, sx, sy, sz, mxy, mxz,
24822 myx, myz, mzx, mzy);
24823
24824
24825 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24826 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24827 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24828 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24829 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24830 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24831 final Acceleration bx2 = new Acceleration(0.0,
24832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24833 calibrator.getBiasXAsAcceleration(bx2);
24834 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24835 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24836 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24837 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24838 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24839 final Acceleration by2 = new Acceleration(0.0,
24840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24841 calibrator.getBiasYAsAcceleration(by2);
24842 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24843 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24844 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24845 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24846 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24847 final Acceleration bz2 = new Acceleration(0.0,
24848 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24849 calibrator.getBiasZAsAcceleration(bz2);
24850 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24851 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24852 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24853 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24854 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24855 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24856 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24857 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24858 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24859 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24860 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24861 final double[] bias1 = calibrator.getBias();
24862 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24863 final double[] bias2 = new double[3];
24864 calibrator.getBias(bias2);
24865 assertArrayEquals(bias1, bias2, 0.0);
24866 final Matrix b1 = calibrator.getBiasAsMatrix();
24867 assertEquals(b1, ba);
24868 final Matrix b2 = new Matrix(3, 1);
24869 calibrator.getBiasAsMatrix(b2);
24870 assertEquals(b1, b2);
24871 final Matrix ma1 = new Matrix(3, 3);
24872 ma1.setSubmatrix(0, 0,
24873 2, 2,
24874 new double[]{sx, myx, mzx,
24875 mxy, sy, mzy,
24876 mxz, myz, sz});
24877 assertEquals(calibrator.getInitialMa(), ma1);
24878 final Matrix ma2 = new Matrix(3, 3);
24879 calibrator.getInitialMa(ma2);
24880 assertEquals(ma1, ma2);
24881 assertNull(calibrator.getMeasurements());
24882 assertTrue(calibrator.isCommonAxisUsed());
24883 assertNull(calibrator.getListener());
24884 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24885 assertFalse(calibrator.isReady());
24886 assertFalse(calibrator.isRunning());
24887 assertNull(calibrator.getEstimatedMa());
24888 assertNull(calibrator.getEstimatedSx());
24889 assertNull(calibrator.getEstimatedSy());
24890 assertNull(calibrator.getEstimatedSz());
24891 assertNull(calibrator.getEstimatedMxy());
24892 assertNull(calibrator.getEstimatedMxz());
24893 assertNull(calibrator.getEstimatedMyx());
24894 assertNull(calibrator.getEstimatedMyz());
24895 assertNull(calibrator.getEstimatedMzx());
24896 assertNull(calibrator.getEstimatedMzy());
24897 assertNull(calibrator.getEstimatedCovariance());
24898 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24899 assertNotNull(calibrator.getGroundTruthGravityNorm());
24900 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24901 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24902 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24903 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24904 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24905 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24906 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24907
24908
24909 final Acceleration invalidGravityNorm = new Acceleration(
24910 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24911
24912 calibrator = null;
24913 try {
24914 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24915 invalidGravityNorm, true,
24916 bx, by, bz, sx, sy, sz, mxy, mxz,
24917 myx, myz, mzx, mzy);
24918 fail("IllegalArgumentException expected but not thrown");
24919 } catch (final IllegalArgumentException ignore) {
24920 }
24921 assertNull(calibrator);
24922 }
24923
24924 @Test
24925 public void testConstructor208() throws WrongSizeException {
24926 final Matrix ba = generateBa();
24927 final double biasX = ba.getElementAtIndex(0);
24928 final double biasY = ba.getElementAtIndex(1);
24929 final double biasZ = ba.getElementAtIndex(2);
24930
24931 final Acceleration bx = new Acceleration(biasX,
24932 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24933 final Acceleration by = new Acceleration(biasY,
24934 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24935 final Acceleration bz = new Acceleration(biasZ,
24936 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24937
24938 final Matrix ma = generateMaCommonAxis();
24939 final double sx = ma.getElementAt(0, 0);
24940 final double sy = ma.getElementAt(1, 1);
24941 final double sz = ma.getElementAt(2, 2);
24942 final double mxy = ma.getElementAt(0, 1);
24943 final double mxz = ma.getElementAt(0, 2);
24944 final double myx = ma.getElementAt(1, 0);
24945 final double myz = ma.getElementAt(1, 2);
24946 final double mzx = ma.getElementAt(2, 0);
24947 final double mzy = ma.getElementAt(2, 1);
24948
24949 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24950 final double latitude = Math.toRadians(
24951 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24952 final double longitude = Math.toRadians(
24953 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24954 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24955 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24956 final NEDVelocity nedVelocity = new NEDVelocity();
24957 final ECEFPosition ecefPosition = new ECEFPosition();
24958 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24959 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24960 ecefPosition, ecefVelocity);
24961 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24962 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24963 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24964
24965 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24966 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24967 true, bx, by, bz, sx, sy, sz, mxy, mxz,
24968 myx, myz, mzx, mzy, this);
24969
24970
24971 assertEquals(calibrator.getBiasX(), biasX, 0.0);
24972 assertEquals(calibrator.getBiasY(), biasY, 0.0);
24973 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24974 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24975 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24976 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24977 final Acceleration bx2 = new Acceleration(0.0,
24978 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24979 calibrator.getBiasXAsAcceleration(bx2);
24980 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24981 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24982 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24983 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24984 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24985 final Acceleration by2 = new Acceleration(0.0,
24986 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24987 calibrator.getBiasYAsAcceleration(by2);
24988 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24989 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24990 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24991 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24992 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24993 final Acceleration bz2 = new Acceleration(0.0,
24994 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24995 calibrator.getBiasZAsAcceleration(bz2);
24996 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24997 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24998 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24999 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25000 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25001 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25002 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25003 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25004 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25005 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25006 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25007 final double[] bias1 = calibrator.getBias();
25008 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25009 final double[] bias2 = new double[3];
25010 calibrator.getBias(bias2);
25011 assertArrayEquals(bias1, bias2, 0.0);
25012 final Matrix b1 = calibrator.getBiasAsMatrix();
25013 assertEquals(b1, ba);
25014 final Matrix b2 = new Matrix(3, 1);
25015 calibrator.getBiasAsMatrix(b2);
25016 assertEquals(b1, b2);
25017 final Matrix ma1 = new Matrix(3, 3);
25018 ma1.setSubmatrix(0, 0,
25019 2, 2,
25020 new double[]{sx, myx, mzx,
25021 mxy, sy, mzy,
25022 mxz, myz, sz});
25023 assertEquals(calibrator.getInitialMa(), ma1);
25024 final Matrix ma2 = new Matrix(3, 3);
25025 calibrator.getInitialMa(ma2);
25026 assertEquals(ma1, ma2);
25027 assertNull(calibrator.getMeasurements());
25028 assertTrue(calibrator.isCommonAxisUsed());
25029 assertSame(calibrator.getListener(), this);
25030 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25031 assertFalse(calibrator.isReady());
25032 assertFalse(calibrator.isRunning());
25033 assertNull(calibrator.getEstimatedMa());
25034 assertNull(calibrator.getEstimatedSx());
25035 assertNull(calibrator.getEstimatedSy());
25036 assertNull(calibrator.getEstimatedSz());
25037 assertNull(calibrator.getEstimatedMxy());
25038 assertNull(calibrator.getEstimatedMxz());
25039 assertNull(calibrator.getEstimatedMyx());
25040 assertNull(calibrator.getEstimatedMyz());
25041 assertNull(calibrator.getEstimatedMzx());
25042 assertNull(calibrator.getEstimatedMzy());
25043 assertNull(calibrator.getEstimatedCovariance());
25044 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25045 assertNotNull(calibrator.getGroundTruthGravityNorm());
25046 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25047 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25048 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25049 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25050 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25051 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25052 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25053
25054
25055 final Acceleration invalidGravityNorm = new Acceleration(
25056 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25057
25058 calibrator = null;
25059 try {
25060 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25061 invalidGravityNorm, true,
25062 bx, by, bz, sx, sy, sz, mxy, mxz,
25063 myx, myz, mzx, mzy, this);
25064 fail("IllegalArgumentException expected but not thrown");
25065 } catch (final IllegalArgumentException ignore) {
25066 }
25067 assertNull(calibrator);
25068 }
25069
25070 @Test
25071 public void testConstructor209() throws WrongSizeException {
25072 final Collection<StandardDeviationBodyKinematics> measurements =
25073 Collections.emptyList();
25074
25075 final Matrix ba = generateBa();
25076 final double biasX = ba.getElementAtIndex(0);
25077 final double biasY = ba.getElementAtIndex(1);
25078 final double biasZ = ba.getElementAtIndex(2);
25079
25080 final Acceleration bx = new Acceleration(biasX,
25081 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25082 final Acceleration by = new Acceleration(biasY,
25083 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25084 final Acceleration bz = new Acceleration(biasZ,
25085 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25086
25087 final Matrix ma = generateMaCommonAxis();
25088 final double sx = ma.getElementAt(0, 0);
25089 final double sy = ma.getElementAt(1, 1);
25090 final double sz = ma.getElementAt(2, 2);
25091 final double mxy = ma.getElementAt(0, 1);
25092 final double mxz = ma.getElementAt(0, 2);
25093 final double myx = ma.getElementAt(1, 0);
25094 final double myz = ma.getElementAt(1, 2);
25095 final double mzx = ma.getElementAt(2, 0);
25096 final double mzy = ma.getElementAt(2, 1);
25097
25098 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25099 final double latitude = Math.toRadians(
25100 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25101 final double longitude = Math.toRadians(
25102 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25103 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25104 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25105 final NEDVelocity nedVelocity = new NEDVelocity();
25106 final ECEFPosition ecefPosition = new ECEFPosition();
25107 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25108 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25109 ecefPosition, ecefVelocity);
25110 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25111 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25112 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25113
25114 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25115 new KnownBiasAndGravityNormAccelerometerCalibrator(
25116 gravityNorm, measurements,
25117 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25118 myx, myz, mzx, mzy);
25119
25120
25121 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25122 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25123 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25124 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25125 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25126 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25127 final Acceleration bx2 = new Acceleration(0.0,
25128 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25129 calibrator.getBiasXAsAcceleration(bx2);
25130 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25131 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25132 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25133 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25134 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25135 final Acceleration by2 = new Acceleration(0.0,
25136 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25137 calibrator.getBiasYAsAcceleration(by2);
25138 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25139 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25140 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25141 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25142 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25143 final Acceleration bz2 = new Acceleration(0.0,
25144 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25145 calibrator.getBiasZAsAcceleration(bz2);
25146 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25147 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25148 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25149 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25150 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25151 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25152 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25153 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25154 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25155 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25156 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25157 final double[] bias1 = calibrator.getBias();
25158 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25159 final double[] bias2 = new double[3];
25160 calibrator.getBias(bias2);
25161 assertArrayEquals(bias1, bias2, 0.0);
25162 final Matrix b1 = calibrator.getBiasAsMatrix();
25163 assertEquals(b1, ba);
25164 final Matrix b2 = new Matrix(3, 1);
25165 calibrator.getBiasAsMatrix(b2);
25166 assertEquals(b1, b2);
25167 final Matrix ma1 = new Matrix(3, 3);
25168 ma1.setSubmatrix(0, 0,
25169 2, 2,
25170 new double[]{sx, myx, mzx,
25171 mxy, sy, mzy,
25172 mxz, myz, sz});
25173 assertEquals(calibrator.getInitialMa(), ma1);
25174 final Matrix ma2 = new Matrix(3, 3);
25175 calibrator.getInitialMa(ma2);
25176 assertEquals(ma1, ma2);
25177 assertSame(calibrator.getMeasurements(), measurements);
25178 assertTrue(calibrator.isCommonAxisUsed());
25179 assertNull(calibrator.getListener());
25180 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25181 assertFalse(calibrator.isReady());
25182 assertFalse(calibrator.isRunning());
25183 assertNull(calibrator.getEstimatedMa());
25184 assertNull(calibrator.getEstimatedSx());
25185 assertNull(calibrator.getEstimatedSy());
25186 assertNull(calibrator.getEstimatedSz());
25187 assertNull(calibrator.getEstimatedMxy());
25188 assertNull(calibrator.getEstimatedMxz());
25189 assertNull(calibrator.getEstimatedMyx());
25190 assertNull(calibrator.getEstimatedMyz());
25191 assertNull(calibrator.getEstimatedMzx());
25192 assertNull(calibrator.getEstimatedMzy());
25193 assertNull(calibrator.getEstimatedCovariance());
25194 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25195 assertNotNull(calibrator.getGroundTruthGravityNorm());
25196 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25197 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25198 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25199 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25200 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25201 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25202 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25203
25204
25205 final Acceleration invalidGravityNorm = new Acceleration(
25206 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25207
25208 calibrator = null;
25209 try {
25210 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25211 invalidGravityNorm, measurements,
25212 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25213 myx, myz, mzx, mzy);
25214 fail("IllegalArgumentException expected but not thrown");
25215 } catch (final IllegalArgumentException ignore) {
25216 }
25217 assertNull(calibrator);
25218 }
25219
25220 @Test
25221 public void testConstructor210() throws WrongSizeException {
25222 final Collection<StandardDeviationBodyKinematics> measurements =
25223 Collections.emptyList();
25224
25225 final Matrix ba = generateBa();
25226 final double biasX = ba.getElementAtIndex(0);
25227 final double biasY = ba.getElementAtIndex(1);
25228 final double biasZ = ba.getElementAtIndex(2);
25229
25230 final Acceleration bx = new Acceleration(biasX,
25231 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25232 final Acceleration by = new Acceleration(biasY,
25233 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25234 final Acceleration bz = new Acceleration(biasZ,
25235 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25236
25237 final Matrix ma = generateMaCommonAxis();
25238 final double sx = ma.getElementAt(0, 0);
25239 final double sy = ma.getElementAt(1, 1);
25240 final double sz = ma.getElementAt(2, 2);
25241 final double mxy = ma.getElementAt(0, 1);
25242 final double mxz = ma.getElementAt(0, 2);
25243 final double myx = ma.getElementAt(1, 0);
25244 final double myz = ma.getElementAt(1, 2);
25245 final double mzx = ma.getElementAt(2, 0);
25246 final double mzy = ma.getElementAt(2, 1);
25247
25248 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25249 final double latitude = Math.toRadians(
25250 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25251 final double longitude = Math.toRadians(
25252 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25253 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25254 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25255 final NEDVelocity nedVelocity = new NEDVelocity();
25256 final ECEFPosition ecefPosition = new ECEFPosition();
25257 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25258 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25259 ecefPosition, ecefVelocity);
25260 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25261 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25262 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25263
25264 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25265 new KnownBiasAndGravityNormAccelerometerCalibrator(
25266 gravityNorm, measurements,
25267 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25268 myx, myz, mzx, mzy, this);
25269
25270
25271 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25272 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25273 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25274 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25275 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25276 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25277 final Acceleration bx2 = new Acceleration(0.0,
25278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25279 calibrator.getBiasXAsAcceleration(bx2);
25280 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25281 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25282 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25283 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25284 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25285 final Acceleration by2 = new Acceleration(0.0,
25286 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25287 calibrator.getBiasYAsAcceleration(by2);
25288 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25289 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25290 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25291 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25292 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25293 final Acceleration bz2 = new Acceleration(0.0,
25294 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25295 calibrator.getBiasZAsAcceleration(bz2);
25296 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25297 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25298 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25299 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25300 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25301 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25302 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25303 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25304 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25305 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25306 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25307 final double[] bias1 = calibrator.getBias();
25308 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25309 final double[] bias2 = new double[3];
25310 calibrator.getBias(bias2);
25311 assertArrayEquals(bias1, bias2, 0.0);
25312 final Matrix b1 = calibrator.getBiasAsMatrix();
25313 assertEquals(b1, ba);
25314 final Matrix b2 = new Matrix(3, 1);
25315 calibrator.getBiasAsMatrix(b2);
25316 assertEquals(b1, b2);
25317 final Matrix ma1 = new Matrix(3, 3);
25318 ma1.setSubmatrix(0, 0,
25319 2, 2,
25320 new double[]{sx, myx, mzx,
25321 mxy, sy, mzy,
25322 mxz, myz, sz});
25323 assertEquals(calibrator.getInitialMa(), ma1);
25324 final Matrix ma2 = new Matrix(3, 3);
25325 calibrator.getInitialMa(ma2);
25326 assertEquals(ma1, ma2);
25327 assertSame(calibrator.getMeasurements(), measurements);
25328 assertTrue(calibrator.isCommonAxisUsed());
25329 assertSame(calibrator.getListener(), this);
25330 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25331 assertFalse(calibrator.isReady());
25332 assertFalse(calibrator.isRunning());
25333 assertNull(calibrator.getEstimatedMa());
25334 assertNull(calibrator.getEstimatedSx());
25335 assertNull(calibrator.getEstimatedSy());
25336 assertNull(calibrator.getEstimatedSz());
25337 assertNull(calibrator.getEstimatedMxy());
25338 assertNull(calibrator.getEstimatedMxz());
25339 assertNull(calibrator.getEstimatedMyx());
25340 assertNull(calibrator.getEstimatedMyz());
25341 assertNull(calibrator.getEstimatedMzx());
25342 assertNull(calibrator.getEstimatedMzy());
25343 assertNull(calibrator.getEstimatedCovariance());
25344 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25345 assertNotNull(calibrator.getGroundTruthGravityNorm());
25346 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25347 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25348 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25349 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25350 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25351 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25352 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25353
25354
25355 final Acceleration invalidGravityNorm = new Acceleration(
25356 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25357
25358 calibrator = null;
25359 try {
25360 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25361 invalidGravityNorm, measurements,
25362 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25363 myx, myz, mzx, mzy, this);
25364 fail("IllegalArgumentException expected but not thrown");
25365 } catch (final IllegalArgumentException ignore) {
25366 }
25367 assertNull(calibrator);
25368 }
25369
25370 @Test
25371 public void testConstructor211() throws WrongSizeException {
25372 final Matrix ba = generateBa();
25373 final double[] bias = ba.getBuffer();
25374 final double biasX = ba.getElementAtIndex(0);
25375 final double biasY = ba.getElementAtIndex(1);
25376 final double biasZ = ba.getElementAtIndex(2);
25377
25378 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25379 final double latitude = Math.toRadians(
25380 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25381 final double longitude = Math.toRadians(
25382 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25383 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25384 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25385 final NEDVelocity nedVelocity = new NEDVelocity();
25386 final ECEFPosition ecefPosition = new ECEFPosition();
25387 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25388 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25389 ecefPosition, ecefVelocity);
25390 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25391 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25392 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25393
25394 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25395 new KnownBiasAndGravityNormAccelerometerCalibrator(
25396 gravityNorm, bias);
25397
25398
25399 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25400 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25401 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25402 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25403 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25404 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25405 final Acceleration bx2 = new Acceleration(0.0,
25406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25407 calibrator.getBiasXAsAcceleration(bx2);
25408 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25409 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25410 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25411 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25412 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25413 final Acceleration by2 = new Acceleration(0.0,
25414 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25415 calibrator.getBiasYAsAcceleration(by2);
25416 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25417 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25418 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25419 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25420 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25421 final Acceleration bz2 = new Acceleration(0.0,
25422 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25423 calibrator.getBiasZAsAcceleration(bz2);
25424 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25425 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25426 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25427 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25428 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25429 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25430 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25431 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25432 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25433 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25434 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25435 final double[] bias1 = calibrator.getBias();
25436 assertArrayEquals(bias1, bias, 0.0);
25437 final double[] bias2 = new double[3];
25438 calibrator.getBias(bias2);
25439 assertArrayEquals(bias1, bias2, 0.0);
25440 final Matrix b1 = calibrator.getBiasAsMatrix();
25441 assertEquals(b1, ba);
25442 final Matrix b2 = new Matrix(3, 1);
25443 calibrator.getBiasAsMatrix(b2);
25444 assertEquals(b1, b2);
25445 final Matrix ma1 = calibrator.getInitialMa();
25446 assertEquals(ma1, new Matrix(3, 3));
25447 final Matrix ma2 = new Matrix(3, 3);
25448 calibrator.getInitialMa(ma2);
25449 assertEquals(ma1, ma2);
25450 assertNull(calibrator.getMeasurements());
25451 assertFalse(calibrator.isCommonAxisUsed());
25452 assertNull(calibrator.getListener());
25453 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25454 assertFalse(calibrator.isReady());
25455 assertFalse(calibrator.isRunning());
25456 assertNull(calibrator.getEstimatedMa());
25457 assertNull(calibrator.getEstimatedSx());
25458 assertNull(calibrator.getEstimatedSy());
25459 assertNull(calibrator.getEstimatedSz());
25460 assertNull(calibrator.getEstimatedMxy());
25461 assertNull(calibrator.getEstimatedMxz());
25462 assertNull(calibrator.getEstimatedMyx());
25463 assertNull(calibrator.getEstimatedMyz());
25464 assertNull(calibrator.getEstimatedMzx());
25465 assertNull(calibrator.getEstimatedMzy());
25466 assertNull(calibrator.getEstimatedCovariance());
25467 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25468 assertNotNull(calibrator.getGroundTruthGravityNorm());
25469 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25470 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25471 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25472 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25473 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25474 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25475 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25476
25477
25478 final Acceleration invalidGravityNorm = new Acceleration(
25479 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25480
25481 calibrator = null;
25482 try {
25483 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25484 invalidGravityNorm, bias);
25485 fail("IllegalArgumentException expected but not thrown");
25486 } catch (final IllegalArgumentException ignore) {
25487 }
25488 try {
25489 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25490 gravityNorm, new double[1]);
25491 fail("IllegalArgumentException expected but not thrown");
25492 } catch (final IllegalArgumentException ignore) {
25493 }
25494 assertNull(calibrator);
25495 }
25496
25497 @Test
25498 public void testConstructor212() throws WrongSizeException {
25499 final Matrix ba = generateBa();
25500 final double[] bias = ba.getBuffer();
25501 final double biasX = ba.getElementAtIndex(0);
25502 final double biasY = ba.getElementAtIndex(1);
25503 final double biasZ = ba.getElementAtIndex(2);
25504
25505 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25506 final double latitude = Math.toRadians(
25507 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25508 final double longitude = Math.toRadians(
25509 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25510 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25511 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25512 final NEDVelocity nedVelocity = new NEDVelocity();
25513 final ECEFPosition ecefPosition = new ECEFPosition();
25514 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25515 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25516 ecefPosition, ecefVelocity);
25517 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25518 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25519 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25520
25521 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25522 new KnownBiasAndGravityNormAccelerometerCalibrator(
25523 gravityNorm, bias, this);
25524
25525
25526 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25527 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25528 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25529 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25530 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25531 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25532 final Acceleration bx2 = new Acceleration(0.0,
25533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25534 calibrator.getBiasXAsAcceleration(bx2);
25535 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25536 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25537 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25538 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25539 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25540 final Acceleration by2 = new Acceleration(0.0,
25541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25542 calibrator.getBiasYAsAcceleration(by2);
25543 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25544 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25545 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25546 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25547 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25548 final Acceleration bz2 = new Acceleration(0.0,
25549 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25550 calibrator.getBiasZAsAcceleration(bz2);
25551 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25552 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25553 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25554 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25555 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25556 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25557 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25558 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25559 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25560 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25561 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25562 final double[] bias1 = calibrator.getBias();
25563 assertArrayEquals(bias1, bias, 0.0);
25564 final double[] bias2 = new double[3];
25565 calibrator.getBias(bias2);
25566 assertArrayEquals(bias1, bias2, 0.0);
25567 final Matrix b1 = calibrator.getBiasAsMatrix();
25568 assertEquals(b1, ba);
25569 final Matrix b2 = new Matrix(3, 1);
25570 calibrator.getBiasAsMatrix(b2);
25571 assertEquals(b1, b2);
25572 final Matrix ma1 = calibrator.getInitialMa();
25573 assertEquals(ma1, new Matrix(3, 3));
25574 final Matrix ma2 = new Matrix(3, 3);
25575 calibrator.getInitialMa(ma2);
25576 assertEquals(ma1, ma2);
25577 assertNull(calibrator.getMeasurements());
25578 assertFalse(calibrator.isCommonAxisUsed());
25579 assertSame(calibrator.getListener(), this);
25580 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25581 assertFalse(calibrator.isReady());
25582 assertFalse(calibrator.isRunning());
25583 assertNull(calibrator.getEstimatedMa());
25584 assertNull(calibrator.getEstimatedSx());
25585 assertNull(calibrator.getEstimatedSy());
25586 assertNull(calibrator.getEstimatedSz());
25587 assertNull(calibrator.getEstimatedMxy());
25588 assertNull(calibrator.getEstimatedMxz());
25589 assertNull(calibrator.getEstimatedMyx());
25590 assertNull(calibrator.getEstimatedMyz());
25591 assertNull(calibrator.getEstimatedMzx());
25592 assertNull(calibrator.getEstimatedMzy());
25593 assertNull(calibrator.getEstimatedCovariance());
25594 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25595 assertNotNull(calibrator.getGroundTruthGravityNorm());
25596 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25597 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25598 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25599 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25600 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25601 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25602 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25603
25604
25605 final Acceleration invalidGravityNorm = new Acceleration(
25606 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25607
25608 calibrator = null;
25609 try {
25610 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25611 invalidGravityNorm, bias, this);
25612 fail("IllegalArgumentException expected but not thrown");
25613 } catch (final IllegalArgumentException ignore) {
25614 }
25615 try {
25616 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25617 gravityNorm, new double[1], this);
25618 fail("IllegalArgumentException expected but not thrown");
25619 } catch (final IllegalArgumentException ignore) {
25620 }
25621 assertNull(calibrator);
25622 }
25623
25624 @Test
25625 public void testConstructor213() throws WrongSizeException {
25626 final Collection<StandardDeviationBodyKinematics> measurements =
25627 Collections.emptyList();
25628
25629 final Matrix ba = generateBa();
25630 final double[] bias = ba.getBuffer();
25631 final double biasX = ba.getElementAtIndex(0);
25632 final double biasY = ba.getElementAtIndex(1);
25633 final double biasZ = ba.getElementAtIndex(2);
25634
25635 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25636 final double latitude = Math.toRadians(
25637 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25638 final double longitude = Math.toRadians(
25639 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25640 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25641 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25642 final NEDVelocity nedVelocity = new NEDVelocity();
25643 final ECEFPosition ecefPosition = new ECEFPosition();
25644 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25645 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25646 ecefPosition, ecefVelocity);
25647 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25648 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25649 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25650
25651 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25652 new KnownBiasAndGravityNormAccelerometerCalibrator(
25653 gravityNorm, measurements, bias);
25654
25655
25656 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25657 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25658 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25659 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25660 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25661 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25662 final Acceleration bx2 = new Acceleration(0.0,
25663 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25664 calibrator.getBiasXAsAcceleration(bx2);
25665 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25666 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25667 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25668 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25669 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25670 final Acceleration by2 = new Acceleration(0.0,
25671 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25672 calibrator.getBiasYAsAcceleration(by2);
25673 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25674 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25675 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25676 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25677 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25678 final Acceleration bz2 = new Acceleration(0.0,
25679 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25680 calibrator.getBiasZAsAcceleration(bz2);
25681 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25682 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25683 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25684 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25685 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25686 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25687 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25688 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25689 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25690 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25691 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25692 final double[] bias1 = calibrator.getBias();
25693 assertArrayEquals(bias1, bias, 0.0);
25694 final double[] bias2 = new double[3];
25695 calibrator.getBias(bias2);
25696 assertArrayEquals(bias1, bias2, 0.0);
25697 final Matrix b1 = calibrator.getBiasAsMatrix();
25698 assertEquals(b1, ba);
25699 final Matrix b2 = new Matrix(3, 1);
25700 calibrator.getBiasAsMatrix(b2);
25701 assertEquals(b1, b2);
25702 final Matrix ma1 = calibrator.getInitialMa();
25703 assertEquals(ma1, new Matrix(3, 3));
25704 final Matrix ma2 = new Matrix(3, 3);
25705 calibrator.getInitialMa(ma2);
25706 assertEquals(ma1, ma2);
25707 assertSame(calibrator.getMeasurements(), measurements);
25708 assertFalse(calibrator.isCommonAxisUsed());
25709 assertNull(calibrator.getListener());
25710 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25711 assertFalse(calibrator.isReady());
25712 assertFalse(calibrator.isRunning());
25713 assertNull(calibrator.getEstimatedMa());
25714 assertNull(calibrator.getEstimatedSx());
25715 assertNull(calibrator.getEstimatedSy());
25716 assertNull(calibrator.getEstimatedSz());
25717 assertNull(calibrator.getEstimatedMxy());
25718 assertNull(calibrator.getEstimatedMxz());
25719 assertNull(calibrator.getEstimatedMyx());
25720 assertNull(calibrator.getEstimatedMyz());
25721 assertNull(calibrator.getEstimatedMzx());
25722 assertNull(calibrator.getEstimatedMzy());
25723 assertNull(calibrator.getEstimatedCovariance());
25724 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25725 assertNotNull(calibrator.getGroundTruthGravityNorm());
25726 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25727 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25728 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25729 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25730 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25731 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25732 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25733
25734
25735 final Acceleration invalidGravityNorm = new Acceleration(
25736 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25737
25738 calibrator = null;
25739 try {
25740 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25741 invalidGravityNorm, measurements, bias);
25742 fail("IllegalArgumentException expected but not thrown");
25743 } catch (final IllegalArgumentException ignore) {
25744 }
25745 try {
25746 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25747 gravityNorm, measurements, new double[1]);
25748 fail("IllegalArgumentException expected but not thrown");
25749 } catch (final IllegalArgumentException ignore) {
25750 }
25751 assertNull(calibrator);
25752 }
25753
25754 @Test
25755 public void testConstructor214() throws WrongSizeException {
25756 final Collection<StandardDeviationBodyKinematics> measurements =
25757 Collections.emptyList();
25758
25759 final Matrix ba = generateBa();
25760 final double[] bias = ba.getBuffer();
25761 final double biasX = ba.getElementAtIndex(0);
25762 final double biasY = ba.getElementAtIndex(1);
25763 final double biasZ = ba.getElementAtIndex(2);
25764
25765 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25766 final double latitude = Math.toRadians(
25767 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25768 final double longitude = Math.toRadians(
25769 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25770 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25771 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25772 final NEDVelocity nedVelocity = new NEDVelocity();
25773 final ECEFPosition ecefPosition = new ECEFPosition();
25774 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25775 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25776 ecefPosition, ecefVelocity);
25777 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25778 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25779 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25780
25781 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25782 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
25783 measurements, bias, this);
25784
25785
25786 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25787 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25788 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25789 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25790 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25791 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25792 final Acceleration bx2 = new Acceleration(0.0,
25793 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25794 calibrator.getBiasXAsAcceleration(bx2);
25795 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25796 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25797 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25798 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25799 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25800 final Acceleration by2 = new Acceleration(0.0,
25801 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25802 calibrator.getBiasYAsAcceleration(by2);
25803 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25804 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25805 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25806 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25807 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25808 final Acceleration bz2 = new Acceleration(0.0,
25809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25810 calibrator.getBiasZAsAcceleration(bz2);
25811 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25812 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25813 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25814 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25815 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25816 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25817 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25818 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25819 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25820 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25821 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25822 final double[] bias1 = calibrator.getBias();
25823 assertArrayEquals(bias1, bias, 0.0);
25824 final double[] bias2 = new double[3];
25825 calibrator.getBias(bias2);
25826 assertArrayEquals(bias1, bias2, 0.0);
25827 final Matrix b1 = calibrator.getBiasAsMatrix();
25828 assertEquals(b1, ba);
25829 final Matrix b2 = new Matrix(3, 1);
25830 calibrator.getBiasAsMatrix(b2);
25831 assertEquals(b1, b2);
25832 final Matrix ma1 = calibrator.getInitialMa();
25833 assertEquals(ma1, new Matrix(3, 3));
25834 final Matrix ma2 = new Matrix(3, 3);
25835 calibrator.getInitialMa(ma2);
25836 assertEquals(ma1, ma2);
25837 assertSame(calibrator.getMeasurements(), measurements);
25838 assertFalse(calibrator.isCommonAxisUsed());
25839 assertSame(calibrator.getListener(), this);
25840 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25841 assertFalse(calibrator.isReady());
25842 assertFalse(calibrator.isRunning());
25843 assertNull(calibrator.getEstimatedMa());
25844 assertNull(calibrator.getEstimatedSx());
25845 assertNull(calibrator.getEstimatedSy());
25846 assertNull(calibrator.getEstimatedSz());
25847 assertNull(calibrator.getEstimatedMxy());
25848 assertNull(calibrator.getEstimatedMxz());
25849 assertNull(calibrator.getEstimatedMyx());
25850 assertNull(calibrator.getEstimatedMyz());
25851 assertNull(calibrator.getEstimatedMzx());
25852 assertNull(calibrator.getEstimatedMzy());
25853 assertNull(calibrator.getEstimatedCovariance());
25854 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25855 assertNotNull(calibrator.getGroundTruthGravityNorm());
25856 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25857 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25858 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25859 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25860 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25861 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25862 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25863
25864
25865 final Acceleration invalidGravityNorm = new Acceleration(
25866 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25867
25868 calibrator = null;
25869 try {
25870 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25871 invalidGravityNorm, measurements, bias, this);
25872 fail("IllegalArgumentException expected but not thrown");
25873 } catch (final IllegalArgumentException ignore) {
25874 }
25875 try {
25876 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25877 gravityNorm, measurements,
25878 new double[1], this);
25879 fail("IllegalArgumentException expected but not thrown");
25880 } catch (final IllegalArgumentException ignore) {
25881 }
25882 assertNull(calibrator);
25883 }
25884
25885 @Test
25886 public void testConstructor215() throws WrongSizeException {
25887 final Matrix ba = generateBa();
25888 final double[] bias = ba.getBuffer();
25889 final double biasX = ba.getElementAtIndex(0);
25890 final double biasY = ba.getElementAtIndex(1);
25891 final double biasZ = ba.getElementAtIndex(2);
25892
25893 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25894 final double latitude = Math.toRadians(
25895 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25896 final double longitude = Math.toRadians(
25897 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25898 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25899 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25900 final NEDVelocity nedVelocity = new NEDVelocity();
25901 final ECEFPosition ecefPosition = new ECEFPosition();
25902 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25903 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25904 ecefPosition, ecefVelocity);
25905 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25906 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25907 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25908
25909 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25910 new KnownBiasAndGravityNormAccelerometerCalibrator(
25911 gravityNorm, true, bias);
25912
25913
25914 assertEquals(calibrator.getBiasX(), biasX, 0.0);
25915 assertEquals(calibrator.getBiasY(), biasY, 0.0);
25916 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25917 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25918 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25919 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25920 final Acceleration bx2 = new Acceleration(0.0,
25921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25922 calibrator.getBiasXAsAcceleration(bx2);
25923 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25924 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25925 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25926 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25927 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25928 final Acceleration by2 = new Acceleration(0.0,
25929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25930 calibrator.getBiasYAsAcceleration(by2);
25931 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25932 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25933 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25934 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25935 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25936 final Acceleration bz2 = new Acceleration(0.0,
25937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25938 calibrator.getBiasZAsAcceleration(bz2);
25939 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25940 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25941 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25942 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25943 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25944 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25945 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25946 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25947 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25948 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25949 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25950 final double[] bias1 = calibrator.getBias();
25951 assertArrayEquals(bias1, bias, 0.0);
25952 final double[] bias2 = new double[3];
25953 calibrator.getBias(bias2);
25954 assertArrayEquals(bias1, bias2, 0.0);
25955 final Matrix b1 = calibrator.getBiasAsMatrix();
25956 assertEquals(b1, ba);
25957 final Matrix b2 = new Matrix(3, 1);
25958 calibrator.getBiasAsMatrix(b2);
25959 assertEquals(b1, b2);
25960 final Matrix ma1 = calibrator.getInitialMa();
25961 assertEquals(ma1, new Matrix(3, 3));
25962 final Matrix ma2 = new Matrix(3, 3);
25963 calibrator.getInitialMa(ma2);
25964 assertEquals(ma1, ma2);
25965 assertNull(calibrator.getMeasurements());
25966 assertTrue(calibrator.isCommonAxisUsed());
25967 assertNull(calibrator.getListener());
25968 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25969 assertFalse(calibrator.isReady());
25970 assertFalse(calibrator.isRunning());
25971 assertNull(calibrator.getEstimatedMa());
25972 assertNull(calibrator.getEstimatedSx());
25973 assertNull(calibrator.getEstimatedSy());
25974 assertNull(calibrator.getEstimatedSz());
25975 assertNull(calibrator.getEstimatedMxy());
25976 assertNull(calibrator.getEstimatedMxz());
25977 assertNull(calibrator.getEstimatedMyx());
25978 assertNull(calibrator.getEstimatedMyz());
25979 assertNull(calibrator.getEstimatedMzx());
25980 assertNull(calibrator.getEstimatedMzy());
25981 assertNull(calibrator.getEstimatedCovariance());
25982 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25983 assertNotNull(calibrator.getGroundTruthGravityNorm());
25984 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25985 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25986 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25987 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25988 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25989 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25990 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25991
25992
25993 final Acceleration invalidGravityNorm = new Acceleration(
25994 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25995
25996 calibrator = null;
25997 try {
25998 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25999 invalidGravityNorm, true, bias);
26000 fail("IllegalArgumentException expected but not thrown");
26001 } catch (final IllegalArgumentException ignore) {
26002 }
26003 try {
26004 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26005 gravityNorm, true, new double[1]);
26006 fail("IllegalArgumentException expected but not thrown");
26007 } catch (final IllegalArgumentException ignore) {
26008 }
26009 assertNull(calibrator);
26010 }
26011
26012 @Test
26013 public void testConstructor216() throws WrongSizeException {
26014 final Matrix ba = generateBa();
26015 final double[] bias = ba.getBuffer();
26016 final double biasX = ba.getElementAtIndex(0);
26017 final double biasY = ba.getElementAtIndex(1);
26018 final double biasZ = ba.getElementAtIndex(2);
26019
26020 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26021 final double latitude = Math.toRadians(
26022 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26023 final double longitude = Math.toRadians(
26024 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26025 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26026 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26027 final NEDVelocity nedVelocity = new NEDVelocity();
26028 final ECEFPosition ecefPosition = new ECEFPosition();
26029 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26030 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26031 ecefPosition, ecefVelocity);
26032 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26033 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26034 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26035
26036 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26037 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
26038 true, bias, this);
26039
26040
26041 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26042 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26043 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26044 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26045 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26046 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26047 final Acceleration bx2 = new Acceleration(0.0,
26048 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26049 calibrator.getBiasXAsAcceleration(bx2);
26050 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26051 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26052 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26053 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26054 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26055 final Acceleration by2 = new Acceleration(0.0,
26056 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26057 calibrator.getBiasYAsAcceleration(by2);
26058 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26059 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26060 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26061 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26062 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26063 final Acceleration bz2 = new Acceleration(0.0,
26064 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26065 calibrator.getBiasZAsAcceleration(bz2);
26066 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26067 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26068 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26069 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26070 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26071 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26072 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26073 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26074 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26075 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26076 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26077 final double[] bias1 = calibrator.getBias();
26078 assertArrayEquals(bias1, bias, 0.0);
26079 final double[] bias2 = new double[3];
26080 calibrator.getBias(bias2);
26081 assertArrayEquals(bias1, bias2, 0.0);
26082 final Matrix b1 = calibrator.getBiasAsMatrix();
26083 assertEquals(b1, ba);
26084 final Matrix b2 = new Matrix(3, 1);
26085 calibrator.getBiasAsMatrix(b2);
26086 assertEquals(b1, b2);
26087 final Matrix ma1 = calibrator.getInitialMa();
26088 assertEquals(ma1, new Matrix(3, 3));
26089 final Matrix ma2 = new Matrix(3, 3);
26090 calibrator.getInitialMa(ma2);
26091 assertEquals(ma1, ma2);
26092 assertNull(calibrator.getMeasurements());
26093 assertTrue(calibrator.isCommonAxisUsed());
26094 assertSame(calibrator.getListener(), this);
26095 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26096 assertFalse(calibrator.isReady());
26097 assertFalse(calibrator.isRunning());
26098 assertNull(calibrator.getEstimatedMa());
26099 assertNull(calibrator.getEstimatedSx());
26100 assertNull(calibrator.getEstimatedSy());
26101 assertNull(calibrator.getEstimatedSz());
26102 assertNull(calibrator.getEstimatedMxy());
26103 assertNull(calibrator.getEstimatedMxz());
26104 assertNull(calibrator.getEstimatedMyx());
26105 assertNull(calibrator.getEstimatedMyz());
26106 assertNull(calibrator.getEstimatedMzx());
26107 assertNull(calibrator.getEstimatedMzy());
26108 assertNull(calibrator.getEstimatedCovariance());
26109 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26110 assertNotNull(calibrator.getGroundTruthGravityNorm());
26111 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26112 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26113 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26114 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26115 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26116 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26117 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26118
26119
26120 final Acceleration invalidGravityNorm = new Acceleration(
26121 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26122
26123 calibrator = null;
26124 try {
26125 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26126 invalidGravityNorm, true,
26127 bias, this);
26128 fail("IllegalArgumentException expected but not thrown");
26129 } catch (final IllegalArgumentException ignore) {
26130 }
26131 try {
26132 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26133 gravityNorm, true,
26134 new double[1], this);
26135 fail("IllegalArgumentException expected but not thrown");
26136 } catch (final IllegalArgumentException ignore) {
26137 }
26138 assertNull(calibrator);
26139 }
26140
26141 @Test
26142 public void testConstructor217() throws WrongSizeException {
26143 final Collection<StandardDeviationBodyKinematics> measurements =
26144 Collections.emptyList();
26145
26146 final Matrix ba = generateBa();
26147 final double[] bias = ba.getBuffer();
26148 final double biasX = ba.getElementAtIndex(0);
26149 final double biasY = ba.getElementAtIndex(1);
26150 final double biasZ = ba.getElementAtIndex(2);
26151
26152 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26153 final double latitude = Math.toRadians(
26154 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26155 final double longitude = Math.toRadians(
26156 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26157 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26158 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26159 final NEDVelocity nedVelocity = new NEDVelocity();
26160 final ECEFPosition ecefPosition = new ECEFPosition();
26161 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26162 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26163 ecefPosition, ecefVelocity);
26164 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26165 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26166 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26167
26168 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26169 new KnownBiasAndGravityNormAccelerometerCalibrator(
26170 gravityNorm, measurements,
26171 true, bias);
26172
26173
26174 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26175 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26176 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26177 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26178 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26179 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26180 final Acceleration bx2 = new Acceleration(0.0,
26181 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26182 calibrator.getBiasXAsAcceleration(bx2);
26183 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26184 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26185 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26186 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26187 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26188 final Acceleration by2 = new Acceleration(0.0,
26189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26190 calibrator.getBiasYAsAcceleration(by2);
26191 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26192 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26193 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26194 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26195 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26196 final Acceleration bz2 = new Acceleration(0.0,
26197 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26198 calibrator.getBiasZAsAcceleration(bz2);
26199 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26200 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26201 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26202 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26203 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26204 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26205 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26206 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26207 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26208 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26209 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26210 final double[] bias1 = calibrator.getBias();
26211 assertArrayEquals(bias1, bias, 0.0);
26212 final double[] bias2 = new double[3];
26213 calibrator.getBias(bias2);
26214 assertArrayEquals(bias1, bias2, 0.0);
26215 final Matrix b1 = calibrator.getBiasAsMatrix();
26216 assertEquals(b1, ba);
26217 final Matrix b2 = new Matrix(3, 1);
26218 calibrator.getBiasAsMatrix(b2);
26219 assertEquals(b1, b2);
26220 final Matrix ma1 = calibrator.getInitialMa();
26221 assertEquals(ma1, new Matrix(3, 3));
26222 final Matrix ma2 = new Matrix(3, 3);
26223 calibrator.getInitialMa(ma2);
26224 assertEquals(ma1, ma2);
26225 assertSame(calibrator.getMeasurements(), measurements);
26226 assertTrue(calibrator.isCommonAxisUsed());
26227 assertNull(calibrator.getListener());
26228 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26229 assertFalse(calibrator.isReady());
26230 assertFalse(calibrator.isRunning());
26231 assertNull(calibrator.getEstimatedMa());
26232 assertNull(calibrator.getEstimatedSx());
26233 assertNull(calibrator.getEstimatedSy());
26234 assertNull(calibrator.getEstimatedSz());
26235 assertNull(calibrator.getEstimatedMxy());
26236 assertNull(calibrator.getEstimatedMxz());
26237 assertNull(calibrator.getEstimatedMyx());
26238 assertNull(calibrator.getEstimatedMyz());
26239 assertNull(calibrator.getEstimatedMzx());
26240 assertNull(calibrator.getEstimatedMzy());
26241 assertNull(calibrator.getEstimatedCovariance());
26242 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26243 assertNotNull(calibrator.getGroundTruthGravityNorm());
26244 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26245 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26246 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26247 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26248 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26249 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26250 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26251
26252
26253 final Acceleration invalidGravityNorm = new Acceleration(
26254 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26255
26256 calibrator = null;
26257 try {
26258 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26259 invalidGravityNorm, measurements,
26260 true, bias);
26261 fail("IllegalArgumentException expected but not thrown");
26262 } catch (final IllegalArgumentException ignore) {
26263 }
26264 try {
26265 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26266 gravityNorm, measurements,
26267 true, new double[1]);
26268 fail("IllegalArgumentException expected but not thrown");
26269 } catch (final IllegalArgumentException ignore) {
26270 }
26271 assertNull(calibrator);
26272 }
26273
26274 @Test
26275 public void testConstructor218() throws WrongSizeException {
26276 final Collection<StandardDeviationBodyKinematics> measurements =
26277 Collections.emptyList();
26278
26279 final Matrix ba = generateBa();
26280 final double[] bias = ba.getBuffer();
26281 final double biasX = ba.getElementAtIndex(0);
26282 final double biasY = ba.getElementAtIndex(1);
26283 final double biasZ = ba.getElementAtIndex(2);
26284
26285 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26286 final double latitude = Math.toRadians(
26287 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26288 final double longitude = Math.toRadians(
26289 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26290 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26291 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26292 final NEDVelocity nedVelocity = new NEDVelocity();
26293 final ECEFPosition ecefPosition = new ECEFPosition();
26294 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26295 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26296 ecefPosition, ecefVelocity);
26297 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26298 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26299 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26300
26301 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26302 new KnownBiasAndGravityNormAccelerometerCalibrator(
26303 gravityNorm, measurements,
26304 true, bias, this);
26305
26306
26307 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26308 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26309 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26310 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26311 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26312 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26313 final Acceleration bx2 = new Acceleration(0.0,
26314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26315 calibrator.getBiasXAsAcceleration(bx2);
26316 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26317 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26318 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26319 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26320 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26321 final Acceleration by2 = new Acceleration(0.0,
26322 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26323 calibrator.getBiasYAsAcceleration(by2);
26324 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26325 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26326 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26327 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26328 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26329 final Acceleration bz2 = new Acceleration(0.0,
26330 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26331 calibrator.getBiasZAsAcceleration(bz2);
26332 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26333 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26334 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26335 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26336 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26337 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26338 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26339 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26340 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26341 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26342 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26343 final double[] bias1 = calibrator.getBias();
26344 assertArrayEquals(bias1, bias, 0.0);
26345 final double[] bias2 = new double[3];
26346 calibrator.getBias(bias2);
26347 assertArrayEquals(bias1, bias2, 0.0);
26348 final Matrix b1 = calibrator.getBiasAsMatrix();
26349 assertEquals(b1, ba);
26350 final Matrix b2 = new Matrix(3, 1);
26351 calibrator.getBiasAsMatrix(b2);
26352 assertEquals(b1, b2);
26353 final Matrix ma1 = calibrator.getInitialMa();
26354 assertEquals(ma1, new Matrix(3, 3));
26355 final Matrix ma2 = new Matrix(3, 3);
26356 calibrator.getInitialMa(ma2);
26357 assertEquals(ma1, ma2);
26358 assertSame(calibrator.getMeasurements(), measurements);
26359 assertTrue(calibrator.isCommonAxisUsed());
26360 assertSame(calibrator.getListener(), this);
26361 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26362 assertFalse(calibrator.isReady());
26363 assertFalse(calibrator.isRunning());
26364 assertNull(calibrator.getEstimatedMa());
26365 assertNull(calibrator.getEstimatedSx());
26366 assertNull(calibrator.getEstimatedSy());
26367 assertNull(calibrator.getEstimatedSz());
26368 assertNull(calibrator.getEstimatedMxy());
26369 assertNull(calibrator.getEstimatedMxz());
26370 assertNull(calibrator.getEstimatedMyx());
26371 assertNull(calibrator.getEstimatedMyz());
26372 assertNull(calibrator.getEstimatedMzx());
26373 assertNull(calibrator.getEstimatedMzy());
26374 assertNull(calibrator.getEstimatedCovariance());
26375 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26376 assertNotNull(calibrator.getGroundTruthGravityNorm());
26377 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26378 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26379 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26380 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26381 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26382 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26383 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26384
26385
26386 final Acceleration invalidGravityNorm = new Acceleration(
26387 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26388
26389 calibrator = null;
26390 try {
26391 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26392 invalidGravityNorm, measurements,
26393 true, bias, this);
26394 fail("IllegalArgumentException expected but not thrown");
26395 } catch (final IllegalArgumentException ignore) {
26396 }
26397 try {
26398 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26399 gravityNorm, measurements,
26400 true, new double[1], this);
26401 fail("IllegalArgumentException expected but not thrown");
26402 } catch (final IllegalArgumentException ignore) {
26403 }
26404 assertNull(calibrator);
26405 }
26406
26407 @Test
26408 public void testConstructor219() throws WrongSizeException {
26409 final Matrix ba = generateBa();
26410 final double[] bias = ba.getBuffer();
26411 final double biasX = ba.getElementAtIndex(0);
26412 final double biasY = ba.getElementAtIndex(1);
26413 final double biasZ = ba.getElementAtIndex(2);
26414
26415 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26416 final double latitude = Math.toRadians(
26417 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26418 final double longitude = Math.toRadians(
26419 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26420 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26421 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26422 final NEDVelocity nedVelocity = new NEDVelocity();
26423 final ECEFPosition ecefPosition = new ECEFPosition();
26424 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26425 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26426 ecefPosition, ecefVelocity);
26427 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26428 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26429 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26430
26431 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26432 new KnownBiasAndGravityNormAccelerometerCalibrator(
26433 gravityNorm, ba);
26434
26435
26436 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26437 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26438 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26439 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26440 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26441 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26442 final Acceleration bx2 = new Acceleration(0.0,
26443 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26444 calibrator.getBiasXAsAcceleration(bx2);
26445 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26446 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26447 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26448 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26449 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26450 final Acceleration by2 = new Acceleration(0.0,
26451 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26452 calibrator.getBiasYAsAcceleration(by2);
26453 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26454 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26455 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26456 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26457 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26458 final Acceleration bz2 = new Acceleration(0.0,
26459 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26460 calibrator.getBiasZAsAcceleration(bz2);
26461 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26462 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26463 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26464 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26465 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26466 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26467 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26468 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26469 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26470 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26471 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26472 final double[] bias1 = calibrator.getBias();
26473 assertArrayEquals(bias1, bias, 0.0);
26474 final double[] bias2 = new double[3];
26475 calibrator.getBias(bias2);
26476 assertArrayEquals(bias1, bias2, 0.0);
26477 final Matrix b1 = calibrator.getBiasAsMatrix();
26478 assertEquals(b1, ba);
26479 final Matrix b2 = new Matrix(3, 1);
26480 calibrator.getBiasAsMatrix(b2);
26481 assertEquals(b1, b2);
26482 final Matrix ma1 = calibrator.getInitialMa();
26483 assertEquals(ma1, new Matrix(3, 3));
26484 final Matrix ma2 = new Matrix(3, 3);
26485 calibrator.getInitialMa(ma2);
26486 assertEquals(ma1, ma2);
26487 assertNull(calibrator.getMeasurements());
26488 assertFalse(calibrator.isCommonAxisUsed());
26489 assertNull(calibrator.getListener());
26490 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26491 assertFalse(calibrator.isReady());
26492 assertFalse(calibrator.isRunning());
26493 assertNull(calibrator.getEstimatedMa());
26494 assertNull(calibrator.getEstimatedSx());
26495 assertNull(calibrator.getEstimatedSy());
26496 assertNull(calibrator.getEstimatedSz());
26497 assertNull(calibrator.getEstimatedMxy());
26498 assertNull(calibrator.getEstimatedMxz());
26499 assertNull(calibrator.getEstimatedMyx());
26500 assertNull(calibrator.getEstimatedMyz());
26501 assertNull(calibrator.getEstimatedMzx());
26502 assertNull(calibrator.getEstimatedMzy());
26503 assertNull(calibrator.getEstimatedCovariance());
26504 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26505 assertNotNull(calibrator.getGroundTruthGravityNorm());
26506 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26507 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26508 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26509 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26510 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26511 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26512 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26513
26514
26515 final Acceleration invalidGravityNorm = new Acceleration(
26516 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26517
26518 calibrator = null;
26519 try {
26520 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26521 invalidGravityNorm, ba);
26522 fail("IllegalArgumentException expected but not thrown");
26523 } catch (final IllegalArgumentException ignore) {
26524 }
26525 try {
26526 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26527 gravityNorm,
26528 new Matrix(1, 1));
26529 fail("IllegalArgumentException expected but not thrown");
26530 } catch (final IllegalArgumentException ignore) {
26531 }
26532 try {
26533 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26534 gravityNorm, new Matrix(1, 3));
26535 fail("IllegalArgumentException expected but not thrown");
26536 } catch (final IllegalArgumentException ignore) {
26537 }
26538 assertNull(calibrator);
26539 }
26540
26541 @Test
26542 public void testConstructor220() throws WrongSizeException {
26543 final Matrix ba = generateBa();
26544 final double[] bias = ba.getBuffer();
26545 final double biasX = ba.getElementAtIndex(0);
26546 final double biasY = ba.getElementAtIndex(1);
26547 final double biasZ = ba.getElementAtIndex(2);
26548
26549 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26550 final double latitude = Math.toRadians(
26551 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26552 final double longitude = Math.toRadians(
26553 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26554 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26555 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26556 final NEDVelocity nedVelocity = new NEDVelocity();
26557 final ECEFPosition ecefPosition = new ECEFPosition();
26558 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26559 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26560 ecefPosition, ecefVelocity);
26561 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26562 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26563 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26564
26565 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26566 new KnownBiasAndGravityNormAccelerometerCalibrator(
26567 gravityNorm, ba, this);
26568
26569
26570 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26571 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26572 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26573 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26574 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26575 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26576 final Acceleration bx2 = new Acceleration(0.0,
26577 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26578 calibrator.getBiasXAsAcceleration(bx2);
26579 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26580 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26581 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26582 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26583 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26584 final Acceleration by2 = new Acceleration(0.0,
26585 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26586 calibrator.getBiasYAsAcceleration(by2);
26587 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26588 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26589 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26590 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26591 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26592 final Acceleration bz2 = new Acceleration(0.0,
26593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26594 calibrator.getBiasZAsAcceleration(bz2);
26595 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26596 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26597 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26598 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26599 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26600 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26601 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26602 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26603 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26604 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26605 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26606 final double[] bias1 = calibrator.getBias();
26607 assertArrayEquals(bias1, bias, 0.0);
26608 final double[] bias2 = new double[3];
26609 calibrator.getBias(bias2);
26610 assertArrayEquals(bias1, bias2, 0.0);
26611 final Matrix b1 = calibrator.getBiasAsMatrix();
26612 assertEquals(b1, ba);
26613 final Matrix b2 = new Matrix(3, 1);
26614 calibrator.getBiasAsMatrix(b2);
26615 assertEquals(b1, b2);
26616 final Matrix ma1 = calibrator.getInitialMa();
26617 assertEquals(ma1, new Matrix(3, 3));
26618 final Matrix ma2 = new Matrix(3, 3);
26619 calibrator.getInitialMa(ma2);
26620 assertEquals(ma1, ma2);
26621 assertNull(calibrator.getMeasurements());
26622 assertFalse(calibrator.isCommonAxisUsed());
26623 assertSame(calibrator.getListener(), this);
26624 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26625 assertFalse(calibrator.isReady());
26626 assertFalse(calibrator.isRunning());
26627 assertNull(calibrator.getEstimatedMa());
26628 assertNull(calibrator.getEstimatedSx());
26629 assertNull(calibrator.getEstimatedSy());
26630 assertNull(calibrator.getEstimatedSz());
26631 assertNull(calibrator.getEstimatedMxy());
26632 assertNull(calibrator.getEstimatedMxz());
26633 assertNull(calibrator.getEstimatedMyx());
26634 assertNull(calibrator.getEstimatedMyz());
26635 assertNull(calibrator.getEstimatedMzx());
26636 assertNull(calibrator.getEstimatedMzy());
26637 assertNull(calibrator.getEstimatedCovariance());
26638 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26639 assertNotNull(calibrator.getGroundTruthGravityNorm());
26640 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26641 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26642 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26643 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26644 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26645 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26646 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26647
26648
26649 final Acceleration invalidGravityNorm = new Acceleration(
26650 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26651
26652 calibrator = null;
26653 try {
26654 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26655 invalidGravityNorm, ba);
26656 fail("IllegalArgumentException expected but not thrown");
26657 } catch (final IllegalArgumentException ignore) {
26658 }
26659 try {
26660 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26661 gravityNorm, new Matrix(1, 1),
26662 this);
26663 fail("IllegalArgumentException expected but not thrown");
26664 } catch (final IllegalArgumentException ignore) {
26665 }
26666 try {
26667 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26668 gravityNorm, new Matrix(1, 3),
26669 this);
26670 fail("IllegalArgumentException expected but not thrown");
26671 } catch (final IllegalArgumentException ignore) {
26672 }
26673 assertNull(calibrator);
26674 }
26675
26676 @Test
26677 public void testConstructor221() throws WrongSizeException {
26678 final Collection<StandardDeviationBodyKinematics> measurements =
26679 Collections.emptyList();
26680
26681 final Matrix ba = generateBa();
26682 final double[] bias = ba.getBuffer();
26683 final double biasX = ba.getElementAtIndex(0);
26684 final double biasY = ba.getElementAtIndex(1);
26685 final double biasZ = ba.getElementAtIndex(2);
26686
26687 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26688 final double latitude = Math.toRadians(
26689 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26690 final double longitude = Math.toRadians(
26691 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26692 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26693 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26694 final NEDVelocity nedVelocity = new NEDVelocity();
26695 final ECEFPosition ecefPosition = new ECEFPosition();
26696 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26697 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26698 ecefPosition, ecefVelocity);
26699 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26700 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26701 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26702
26703 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26704 new KnownBiasAndGravityNormAccelerometerCalibrator(
26705 gravityNorm, measurements, ba);
26706
26707
26708 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26709 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26710 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26711 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26712 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26713 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26714 final Acceleration bx2 = new Acceleration(0.0,
26715 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26716 calibrator.getBiasXAsAcceleration(bx2);
26717 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26718 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26719 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26720 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26721 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26722 final Acceleration by2 = new Acceleration(0.0,
26723 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26724 calibrator.getBiasYAsAcceleration(by2);
26725 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26726 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26727 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26728 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26729 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26730 final Acceleration bz2 = new Acceleration(0.0,
26731 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26732 calibrator.getBiasZAsAcceleration(bz2);
26733 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26734 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26735 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26736 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26737 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26738 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26739 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26740 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26741 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26742 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26743 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26744 final double[] bias1 = calibrator.getBias();
26745 assertArrayEquals(bias1, bias, 0.0);
26746 final double[] bias2 = new double[3];
26747 calibrator.getBias(bias2);
26748 assertArrayEquals(bias1, bias2, 0.0);
26749 final Matrix b1 = calibrator.getBiasAsMatrix();
26750 assertEquals(b1, ba);
26751 final Matrix b2 = new Matrix(3, 1);
26752 calibrator.getBiasAsMatrix(b2);
26753 assertEquals(b1, b2);
26754 final Matrix ma1 = calibrator.getInitialMa();
26755 assertEquals(ma1, new Matrix(3, 3));
26756 final Matrix ma2 = new Matrix(3, 3);
26757 calibrator.getInitialMa(ma2);
26758 assertEquals(ma1, ma2);
26759 assertSame(calibrator.getMeasurements(), measurements);
26760 assertFalse(calibrator.isCommonAxisUsed());
26761 assertNull(calibrator.getListener());
26762 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26763 assertFalse(calibrator.isReady());
26764 assertFalse(calibrator.isRunning());
26765 assertNull(calibrator.getEstimatedMa());
26766 assertNull(calibrator.getEstimatedSx());
26767 assertNull(calibrator.getEstimatedSy());
26768 assertNull(calibrator.getEstimatedSz());
26769 assertNull(calibrator.getEstimatedMxy());
26770 assertNull(calibrator.getEstimatedMxz());
26771 assertNull(calibrator.getEstimatedMyx());
26772 assertNull(calibrator.getEstimatedMyz());
26773 assertNull(calibrator.getEstimatedMzx());
26774 assertNull(calibrator.getEstimatedMzy());
26775 assertNull(calibrator.getEstimatedCovariance());
26776 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26777 assertNotNull(calibrator.getGroundTruthGravityNorm());
26778 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26779 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26780 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26781 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26782 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26783 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26784 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26785
26786
26787 final Acceleration invalidGravityNorm = new Acceleration(
26788 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26789
26790 calibrator = null;
26791 try {
26792 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26793 invalidGravityNorm, measurements, ba);
26794 fail("IllegalArgumentException expected but not thrown");
26795 } catch (final IllegalArgumentException ignore) {
26796 }
26797 try {
26798 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26799 gravityNorm, measurements,
26800 new Matrix(1, 1));
26801 fail("IllegalArgumentException expected but not thrown");
26802 } catch (final IllegalArgumentException ignore) {
26803 }
26804 try {
26805 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26806 gravityNorm, measurements,
26807 new Matrix(1, 3));
26808 fail("IllegalArgumentException expected but not thrown");
26809 } catch (final IllegalArgumentException ignore) {
26810 }
26811 assertNull(calibrator);
26812 }
26813
26814 @Test
26815 public void testConstructor222() throws WrongSizeException {
26816 final Collection<StandardDeviationBodyKinematics> measurements =
26817 Collections.emptyList();
26818
26819 final Matrix ba = generateBa();
26820 final double[] bias = ba.getBuffer();
26821 final double biasX = ba.getElementAtIndex(0);
26822 final double biasY = ba.getElementAtIndex(1);
26823 final double biasZ = ba.getElementAtIndex(2);
26824
26825 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26826 final double latitude = Math.toRadians(
26827 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26828 final double longitude = Math.toRadians(
26829 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26830 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26831 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26832 final NEDVelocity nedVelocity = new NEDVelocity();
26833 final ECEFPosition ecefPosition = new ECEFPosition();
26834 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26835 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26836 ecefPosition, ecefVelocity);
26837 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26838 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26839 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26840
26841 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26842 new KnownBiasAndGravityNormAccelerometerCalibrator(
26843 gravityNorm, measurements,
26844 ba, this);
26845
26846
26847 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26848 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26849 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26850 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26851 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26852 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26853 final Acceleration bx2 = new Acceleration(0.0,
26854 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26855 calibrator.getBiasXAsAcceleration(bx2);
26856 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26857 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26858 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26859 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26860 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26861 final Acceleration by2 = new Acceleration(0.0,
26862 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26863 calibrator.getBiasYAsAcceleration(by2);
26864 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26865 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26866 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26867 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26868 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26869 final Acceleration bz2 = new Acceleration(0.0,
26870 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26871 calibrator.getBiasZAsAcceleration(bz2);
26872 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26873 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26874 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26875 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26876 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26877 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26878 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26879 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26880 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26881 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26882 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26883 final double[] bias1 = calibrator.getBias();
26884 assertArrayEquals(bias1, bias, 0.0);
26885 final double[] bias2 = new double[3];
26886 calibrator.getBias(bias2);
26887 assertArrayEquals(bias1, bias2, 0.0);
26888 final Matrix b1 = calibrator.getBiasAsMatrix();
26889 assertEquals(b1, ba);
26890 final Matrix b2 = new Matrix(3, 1);
26891 calibrator.getBiasAsMatrix(b2);
26892 assertEquals(b1, b2);
26893 final Matrix ma1 = calibrator.getInitialMa();
26894 assertEquals(ma1, new Matrix(3, 3));
26895 final Matrix ma2 = new Matrix(3, 3);
26896 calibrator.getInitialMa(ma2);
26897 assertEquals(ma1, ma2);
26898 assertSame(calibrator.getMeasurements(), measurements);
26899 assertFalse(calibrator.isCommonAxisUsed());
26900 assertSame(calibrator.getListener(), this);
26901 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26902 assertFalse(calibrator.isReady());
26903 assertFalse(calibrator.isRunning());
26904 assertNull(calibrator.getEstimatedMa());
26905 assertNull(calibrator.getEstimatedSx());
26906 assertNull(calibrator.getEstimatedSy());
26907 assertNull(calibrator.getEstimatedSz());
26908 assertNull(calibrator.getEstimatedMxy());
26909 assertNull(calibrator.getEstimatedMxz());
26910 assertNull(calibrator.getEstimatedMyx());
26911 assertNull(calibrator.getEstimatedMyz());
26912 assertNull(calibrator.getEstimatedMzx());
26913 assertNull(calibrator.getEstimatedMzy());
26914 assertNull(calibrator.getEstimatedCovariance());
26915 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26916 assertNotNull(calibrator.getGroundTruthGravityNorm());
26917 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26918 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26919 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26920 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26921 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26922 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26923 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26924
26925
26926 final Acceleration invalidGravityNorm = new Acceleration(
26927 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26928
26929 calibrator = null;
26930 try {
26931 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26932 invalidGravityNorm, measurements,
26933 ba, this);
26934 fail("IllegalArgumentException expected but not thrown");
26935 } catch (final IllegalArgumentException ignore) {
26936 }
26937 try {
26938 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26939 gravityNorm, measurements,
26940 new Matrix(1, 1), this);
26941 fail("IllegalArgumentException expected but not thrown");
26942 } catch (final IllegalArgumentException ignore) {
26943 }
26944 try {
26945 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26946 gravityNorm, measurements,
26947 new Matrix(1, 3), this);
26948 fail("IllegalArgumentException expected but not thrown");
26949 } catch (final IllegalArgumentException ignore) {
26950 }
26951 assertNull(calibrator);
26952 }
26953
26954 @Test
26955 public void testConstructor223() throws WrongSizeException {
26956 final Matrix ba = generateBa();
26957 final double[] bias = ba.getBuffer();
26958 final double biasX = ba.getElementAtIndex(0);
26959 final double biasY = ba.getElementAtIndex(1);
26960 final double biasZ = ba.getElementAtIndex(2);
26961
26962 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26963 final double latitude = Math.toRadians(
26964 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26965 final double longitude = Math.toRadians(
26966 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26967 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26968 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26969 final NEDVelocity nedVelocity = new NEDVelocity();
26970 final ECEFPosition ecefPosition = new ECEFPosition();
26971 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26972 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26973 ecefPosition, ecefVelocity);
26974 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26975 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26976 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26977
26978 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26979 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
26980 true, ba);
26981
26982
26983 assertEquals(calibrator.getBiasX(), biasX, 0.0);
26984 assertEquals(calibrator.getBiasY(), biasY, 0.0);
26985 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26986 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26987 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26988 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26989 final Acceleration bx2 = new Acceleration(0.0,
26990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26991 calibrator.getBiasXAsAcceleration(bx2);
26992 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26993 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26994 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26995 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26996 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26997 final Acceleration by2 = new Acceleration(0.0,
26998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26999 calibrator.getBiasYAsAcceleration(by2);
27000 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27001 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27002 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27003 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27004 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27005 final Acceleration bz2 = new Acceleration(0.0,
27006 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27007 calibrator.getBiasZAsAcceleration(bz2);
27008 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27009 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27010 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27011 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27012 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27013 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27014 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27015 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27016 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27017 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27018 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27019 final double[] bias1 = calibrator.getBias();
27020 assertArrayEquals(bias1, bias, 0.0);
27021 final double[] bias2 = new double[3];
27022 calibrator.getBias(bias2);
27023 assertArrayEquals(bias1, bias2, 0.0);
27024 final Matrix b1 = calibrator.getBiasAsMatrix();
27025 assertEquals(b1, ba);
27026 final Matrix b2 = new Matrix(3, 1);
27027 calibrator.getBiasAsMatrix(b2);
27028 assertEquals(b1, b2);
27029 final Matrix ma1 = calibrator.getInitialMa();
27030 assertEquals(ma1, new Matrix(3, 3));
27031 final Matrix ma2 = new Matrix(3, 3);
27032 calibrator.getInitialMa(ma2);
27033 assertEquals(ma1, ma2);
27034 assertNull(calibrator.getMeasurements());
27035 assertTrue(calibrator.isCommonAxisUsed());
27036 assertNull(calibrator.getListener());
27037 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27038 assertFalse(calibrator.isReady());
27039 assertFalse(calibrator.isRunning());
27040 assertNull(calibrator.getEstimatedMa());
27041 assertNull(calibrator.getEstimatedSx());
27042 assertNull(calibrator.getEstimatedSy());
27043 assertNull(calibrator.getEstimatedSz());
27044 assertNull(calibrator.getEstimatedMxy());
27045 assertNull(calibrator.getEstimatedMxz());
27046 assertNull(calibrator.getEstimatedMyx());
27047 assertNull(calibrator.getEstimatedMyz());
27048 assertNull(calibrator.getEstimatedMzx());
27049 assertNull(calibrator.getEstimatedMzy());
27050 assertNull(calibrator.getEstimatedCovariance());
27051 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27052 assertNotNull(calibrator.getGroundTruthGravityNorm());
27053 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27054 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27055 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27056 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27057 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27058 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27059 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27060
27061
27062 final Acceleration invalidGravityNorm = new Acceleration(
27063 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27064
27065 calibrator = null;
27066 try {
27067 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27068 invalidGravityNorm, true, ba);
27069 fail("IllegalArgumentException expected but not thrown");
27070 } catch (final IllegalArgumentException ignore) {
27071 }
27072 try {
27073 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27074 gravityNorm, true,
27075 new Matrix(1, 1));
27076 fail("IllegalArgumentException expected but not thrown");
27077 } catch (final IllegalArgumentException ignore) {
27078 }
27079 try {
27080 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27081 gravityNorm, true,
27082 new Matrix(1, 3));
27083 fail("IllegalArgumentException expected but not thrown");
27084 } catch (final IllegalArgumentException ignore) {
27085 }
27086 assertNull(calibrator);
27087 }
27088
27089 @Test
27090 public void testConstructor224() throws WrongSizeException {
27091 final Matrix ba = generateBa();
27092 final double[] bias = ba.getBuffer();
27093 final double biasX = ba.getElementAtIndex(0);
27094 final double biasY = ba.getElementAtIndex(1);
27095 final double biasZ = ba.getElementAtIndex(2);
27096
27097 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27098 final double latitude = Math.toRadians(
27099 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27100 final double longitude = Math.toRadians(
27101 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27102 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27103 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27104 final NEDVelocity nedVelocity = new NEDVelocity();
27105 final ECEFPosition ecefPosition = new ECEFPosition();
27106 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27107 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27108 ecefPosition, ecefVelocity);
27109 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27110 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27111 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27112
27113 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27114 new KnownBiasAndGravityNormAccelerometerCalibrator(
27115 gravityNorm, true, ba, this);
27116
27117
27118 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27119 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27120 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27121 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27122 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27123 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27124 final Acceleration bx2 = new Acceleration(0.0,
27125 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27126 calibrator.getBiasXAsAcceleration(bx2);
27127 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27128 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27129 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27130 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27131 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27132 final Acceleration by2 = new Acceleration(0.0,
27133 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27134 calibrator.getBiasYAsAcceleration(by2);
27135 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27136 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27137 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27138 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27139 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27140 final Acceleration bz2 = new Acceleration(0.0,
27141 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27142 calibrator.getBiasZAsAcceleration(bz2);
27143 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27144 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27145 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27146 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27147 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27148 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27149 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27150 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27151 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27152 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27153 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27154 final double[] bias1 = calibrator.getBias();
27155 assertArrayEquals(bias1, bias, 0.0);
27156 final double[] bias2 = new double[3];
27157 calibrator.getBias(bias2);
27158 assertArrayEquals(bias1, bias2, 0.0);
27159 final Matrix b1 = calibrator.getBiasAsMatrix();
27160 assertEquals(b1, ba);
27161 final Matrix b2 = new Matrix(3, 1);
27162 calibrator.getBiasAsMatrix(b2);
27163 assertEquals(b1, b2);
27164 final Matrix ma1 = calibrator.getInitialMa();
27165 assertEquals(ma1, new Matrix(3, 3));
27166 final Matrix ma2 = new Matrix(3, 3);
27167 calibrator.getInitialMa(ma2);
27168 assertEquals(ma1, ma2);
27169 assertNull(calibrator.getMeasurements());
27170 assertTrue(calibrator.isCommonAxisUsed());
27171 assertSame(calibrator.getListener(), this);
27172 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27173 assertFalse(calibrator.isReady());
27174 assertFalse(calibrator.isRunning());
27175 assertNull(calibrator.getEstimatedMa());
27176 assertNull(calibrator.getEstimatedSx());
27177 assertNull(calibrator.getEstimatedSy());
27178 assertNull(calibrator.getEstimatedSz());
27179 assertNull(calibrator.getEstimatedMxy());
27180 assertNull(calibrator.getEstimatedMxz());
27181 assertNull(calibrator.getEstimatedMyx());
27182 assertNull(calibrator.getEstimatedMyz());
27183 assertNull(calibrator.getEstimatedMzx());
27184 assertNull(calibrator.getEstimatedMzy());
27185 assertNull(calibrator.getEstimatedCovariance());
27186 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27187 assertNotNull(calibrator.getGroundTruthGravityNorm());
27188 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27189 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27190 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27191 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27192 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27193 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27194 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27195
27196
27197 final Acceleration invalidGravityNorm = new Acceleration(
27198 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27199
27200 calibrator = null;
27201 try {
27202 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27203 invalidGravityNorm, true, ba, this);
27204 fail("IllegalArgumentException expected but not thrown");
27205 } catch (final IllegalArgumentException ignore) {
27206 }
27207 try {
27208 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27209 gravityNorm, true, new Matrix(1, 1),
27210 this);
27211 fail("IllegalArgumentException expected but not thrown");
27212 } catch (final IllegalArgumentException ignore) {
27213 }
27214 try {
27215 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27216 gravityNorm, true, new Matrix(1, 3),
27217 this);
27218 fail("IllegalArgumentException expected but not thrown");
27219 } catch (final IllegalArgumentException ignore) {
27220 }
27221 assertNull(calibrator);
27222 }
27223
27224 @Test
27225 public void testConstructor225() throws WrongSizeException {
27226 final Collection<StandardDeviationBodyKinematics> measurements =
27227 Collections.emptyList();
27228
27229 final Matrix ba = generateBa();
27230 final double[] bias = ba.getBuffer();
27231 final double biasX = ba.getElementAtIndex(0);
27232 final double biasY = ba.getElementAtIndex(1);
27233 final double biasZ = ba.getElementAtIndex(2);
27234
27235 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27236 final double latitude = Math.toRadians(
27237 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27238 final double longitude = Math.toRadians(
27239 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27240 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27241 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27242 final NEDVelocity nedVelocity = new NEDVelocity();
27243 final ECEFPosition ecefPosition = new ECEFPosition();
27244 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27245 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27246 ecefPosition, ecefVelocity);
27247 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27248 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27249 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27250
27251 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27252 new KnownBiasAndGravityNormAccelerometerCalibrator(
27253 gravityNorm, measurements,
27254 true, ba);
27255
27256
27257 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27258 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27259 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27260 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27261 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27262 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27263 final Acceleration bx2 = new Acceleration(0.0,
27264 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27265 calibrator.getBiasXAsAcceleration(bx2);
27266 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27267 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27268 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27269 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27270 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27271 final Acceleration by2 = new Acceleration(0.0,
27272 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27273 calibrator.getBiasYAsAcceleration(by2);
27274 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27275 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27276 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27277 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27278 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27279 final Acceleration bz2 = new Acceleration(0.0,
27280 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27281 calibrator.getBiasZAsAcceleration(bz2);
27282 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27283 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27284 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27285 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27286 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27287 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27288 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27289 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27290 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27291 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27292 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27293 final double[] bias1 = calibrator.getBias();
27294 assertArrayEquals(bias1, bias, 0.0);
27295 final double[] bias2 = new double[3];
27296 calibrator.getBias(bias2);
27297 assertArrayEquals(bias1, bias2, 0.0);
27298 final Matrix b1 = calibrator.getBiasAsMatrix();
27299 assertEquals(b1, ba);
27300 final Matrix b2 = new Matrix(3, 1);
27301 calibrator.getBiasAsMatrix(b2);
27302 assertEquals(b1, b2);
27303 final Matrix ma1 = calibrator.getInitialMa();
27304 assertEquals(ma1, new Matrix(3, 3));
27305 final Matrix ma2 = new Matrix(3, 3);
27306 calibrator.getInitialMa(ma2);
27307 assertEquals(ma1, ma2);
27308 assertSame(calibrator.getMeasurements(), measurements);
27309 assertTrue(calibrator.isCommonAxisUsed());
27310 assertNull(calibrator.getListener());
27311 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27312 assertFalse(calibrator.isReady());
27313 assertFalse(calibrator.isRunning());
27314 assertNull(calibrator.getEstimatedMa());
27315 assertNull(calibrator.getEstimatedSx());
27316 assertNull(calibrator.getEstimatedSy());
27317 assertNull(calibrator.getEstimatedSz());
27318 assertNull(calibrator.getEstimatedMxy());
27319 assertNull(calibrator.getEstimatedMxz());
27320 assertNull(calibrator.getEstimatedMyx());
27321 assertNull(calibrator.getEstimatedMyz());
27322 assertNull(calibrator.getEstimatedMzx());
27323 assertNull(calibrator.getEstimatedMzy());
27324 assertNull(calibrator.getEstimatedCovariance());
27325 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27326 assertNotNull(calibrator.getGroundTruthGravityNorm());
27327 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27328 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27329 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27330 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27331 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27332 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27333 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27334
27335
27336 final Acceleration invalidGravityNorm = new Acceleration(
27337 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27338
27339 calibrator = null;
27340 try {
27341 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27342 invalidGravityNorm, measurements,
27343 true, ba);
27344 fail("IllegalArgumentException expected but not thrown");
27345 } catch (final IllegalArgumentException ignore) {
27346 }
27347 try {
27348 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27349 gravityNorm, measurements, true,
27350 new Matrix(1, 1));
27351 fail("IllegalArgumentException expected but not thrown");
27352 } catch (final IllegalArgumentException ignore) {
27353 }
27354 try {
27355 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27356 gravityNorm, measurements, true,
27357 new Matrix(1, 3));
27358 fail("IllegalArgumentException expected but not thrown");
27359 } catch (final IllegalArgumentException ignore) {
27360 }
27361 assertNull(calibrator);
27362 }
27363
27364 @Test
27365 public void testConstructor226() throws WrongSizeException {
27366 final Collection<StandardDeviationBodyKinematics> measurements =
27367 Collections.emptyList();
27368
27369 final Matrix ba = generateBa();
27370 final double[] bias = ba.getBuffer();
27371 final double biasX = ba.getElementAtIndex(0);
27372 final double biasY = ba.getElementAtIndex(1);
27373 final double biasZ = ba.getElementAtIndex(2);
27374
27375 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27376 final double latitude = Math.toRadians(
27377 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27378 final double longitude = Math.toRadians(
27379 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27380 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27381 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27382 final NEDVelocity nedVelocity = new NEDVelocity();
27383 final ECEFPosition ecefPosition = new ECEFPosition();
27384 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27385 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27386 ecefPosition, ecefVelocity);
27387 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27388 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27389 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27390
27391 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27392 new KnownBiasAndGravityNormAccelerometerCalibrator(
27393 gravityNorm, measurements,
27394 true, ba, this);
27395
27396
27397 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27398 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27399 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27400 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27401 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27402 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27403 final Acceleration bx2 = new Acceleration(0.0,
27404 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27405 calibrator.getBiasXAsAcceleration(bx2);
27406 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27407 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27408 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27409 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27410 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27411 final Acceleration by2 = new Acceleration(0.0,
27412 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27413 calibrator.getBiasYAsAcceleration(by2);
27414 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27415 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27416 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27417 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27418 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27419 final Acceleration bz2 = new Acceleration(0.0,
27420 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27421 calibrator.getBiasZAsAcceleration(bz2);
27422 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27423 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27424 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27425 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27426 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27427 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27428 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27429 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27430 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27431 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27432 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27433 final double[] bias1 = calibrator.getBias();
27434 assertArrayEquals(bias1, bias, 0.0);
27435 final double[] bias2 = new double[3];
27436 calibrator.getBias(bias2);
27437 assertArrayEquals(bias1, bias2, 0.0);
27438 final Matrix b1 = calibrator.getBiasAsMatrix();
27439 assertEquals(b1, ba);
27440 final Matrix b2 = new Matrix(3, 1);
27441 calibrator.getBiasAsMatrix(b2);
27442 assertEquals(b1, b2);
27443 final Matrix ma1 = calibrator.getInitialMa();
27444 assertEquals(ma1, new Matrix(3, 3));
27445 final Matrix ma2 = new Matrix(3, 3);
27446 calibrator.getInitialMa(ma2);
27447 assertEquals(ma1, ma2);
27448 assertSame(calibrator.getMeasurements(), measurements);
27449 assertTrue(calibrator.isCommonAxisUsed());
27450 assertSame(calibrator.getListener(), this);
27451 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27452 assertFalse(calibrator.isReady());
27453 assertFalse(calibrator.isRunning());
27454 assertNull(calibrator.getEstimatedMa());
27455 assertNull(calibrator.getEstimatedSx());
27456 assertNull(calibrator.getEstimatedSy());
27457 assertNull(calibrator.getEstimatedSz());
27458 assertNull(calibrator.getEstimatedMxy());
27459 assertNull(calibrator.getEstimatedMxz());
27460 assertNull(calibrator.getEstimatedMyx());
27461 assertNull(calibrator.getEstimatedMyz());
27462 assertNull(calibrator.getEstimatedMzx());
27463 assertNull(calibrator.getEstimatedMzy());
27464 assertNull(calibrator.getEstimatedCovariance());
27465 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27466 assertNotNull(calibrator.getGroundTruthGravityNorm());
27467 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27468 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27469 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27470 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27471 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27472 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27473 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27474
27475
27476 final Acceleration invalidGravityNorm = new Acceleration(
27477 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27478
27479 calibrator = null;
27480 try {
27481 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27482 invalidGravityNorm, measurements,
27483 true, ba, this);
27484 fail("IllegalArgumentException expected but not thrown");
27485 } catch (final IllegalArgumentException ignore) {
27486 }
27487 try {
27488 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27489 gravityNorm, measurements, true,
27490 new Matrix(1, 1), this);
27491 fail("IllegalArgumentException expected but not thrown");
27492 } catch (final IllegalArgumentException ignore) {
27493 }
27494 try {
27495 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27496 gravityNorm, measurements, true,
27497 new Matrix(1, 3), this);
27498 fail("IllegalArgumentException expected but not thrown");
27499 } catch (final IllegalArgumentException ignore) {
27500 }
27501 assertNull(calibrator);
27502 }
27503
27504 @Test
27505 public void testConstructor227() throws WrongSizeException {
27506 final Matrix ba = generateBa();
27507 final double[] bias = ba.getBuffer();
27508 final double biasX = ba.getElementAtIndex(0);
27509 final double biasY = ba.getElementAtIndex(1);
27510 final double biasZ = ba.getElementAtIndex(2);
27511
27512 final Matrix ma = generateMaCommonAxis();
27513 final double sx = ma.getElementAt(0, 0);
27514 final double sy = ma.getElementAt(1, 1);
27515 final double sz = ma.getElementAt(2, 2);
27516 final double mxy = ma.getElementAt(0, 1);
27517 final double mxz = ma.getElementAt(0, 2);
27518 final double myx = ma.getElementAt(1, 0);
27519 final double myz = ma.getElementAt(1, 2);
27520 final double mzx = ma.getElementAt(2, 0);
27521 final double mzy = ma.getElementAt(2, 1);
27522
27523 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27524 final double latitude = Math.toRadians(
27525 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27526 final double longitude = Math.toRadians(
27527 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27528 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27529 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27530 final NEDVelocity nedVelocity = new NEDVelocity();
27531 final ECEFPosition ecefPosition = new ECEFPosition();
27532 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27533 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27534 ecefPosition, ecefVelocity);
27535 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27536 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27537 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27538
27539 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27540 new KnownBiasAndGravityNormAccelerometerCalibrator(
27541 gravityNorm, ba, ma);
27542
27543
27544 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27545 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27546 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27547 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27548 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27549 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27550 final Acceleration bx2 = new Acceleration(0.0,
27551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27552 calibrator.getBiasXAsAcceleration(bx2);
27553 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27554 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27555 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27556 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27557 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27558 final Acceleration by2 = new Acceleration(0.0,
27559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27560 calibrator.getBiasYAsAcceleration(by2);
27561 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27562 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27563 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27564 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27565 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27566 final Acceleration bz2 = new Acceleration(0.0,
27567 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27568 calibrator.getBiasZAsAcceleration(bz2);
27569 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27570 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27571 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27572 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27573 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27574 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27575 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27576 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27577 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27578 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27579 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27580 final double[] bias1 = calibrator.getBias();
27581 assertArrayEquals(bias1, bias, 0.0);
27582 final double[] bias2 = new double[3];
27583 calibrator.getBias(bias2);
27584 assertArrayEquals(bias1, bias2, 0.0);
27585 final Matrix b1 = calibrator.getBiasAsMatrix();
27586 assertEquals(b1, ba);
27587 final Matrix b2 = new Matrix(3, 1);
27588 calibrator.getBiasAsMatrix(b2);
27589 assertEquals(b1, b2);
27590 final Matrix ma1 = new Matrix(3, 3);
27591 ma1.setSubmatrix(0, 0,
27592 2, 2,
27593 new double[]{sx, myx, mzx,
27594 mxy, sy, mzy,
27595 mxz, myz, sz});
27596 assertEquals(calibrator.getInitialMa(), ma1);
27597 final Matrix ma2 = new Matrix(3, 3);
27598 calibrator.getInitialMa(ma2);
27599 assertEquals(ma1, ma2);
27600 assertNull(calibrator.getMeasurements());
27601 assertFalse(calibrator.isCommonAxisUsed());
27602 assertNull(calibrator.getListener());
27603 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27604 assertFalse(calibrator.isReady());
27605 assertFalse(calibrator.isRunning());
27606 assertNull(calibrator.getEstimatedMa());
27607 assertNull(calibrator.getEstimatedSx());
27608 assertNull(calibrator.getEstimatedSy());
27609 assertNull(calibrator.getEstimatedSz());
27610 assertNull(calibrator.getEstimatedMxy());
27611 assertNull(calibrator.getEstimatedMxz());
27612 assertNull(calibrator.getEstimatedMyx());
27613 assertNull(calibrator.getEstimatedMyz());
27614 assertNull(calibrator.getEstimatedMzx());
27615 assertNull(calibrator.getEstimatedMzy());
27616 assertNull(calibrator.getEstimatedCovariance());
27617 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27618 assertNotNull(calibrator.getGroundTruthGravityNorm());
27619 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27620 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27621 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27622 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27623 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27624 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27625 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27626
27627
27628 final Acceleration invalidGravityNorm = new Acceleration(
27629 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27630
27631 calibrator = null;
27632 try {
27633 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27634 invalidGravityNorm, ba, ma);
27635 fail("IllegalArgumentException expected but not thrown");
27636 } catch (final IllegalArgumentException ignore) {
27637 }
27638 try {
27639 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27640 gravityNorm, new Matrix(1, 1), ma);
27641 fail("IllegalArgumentException expected but not thrown");
27642 } catch (final IllegalArgumentException ignore) {
27643 }
27644 try {
27645 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27646 gravityNorm, new Matrix(1, 3), ma);
27647 fail("IllegalArgumentException expected but not thrown");
27648 } catch (final IllegalArgumentException ignore) {
27649 }
27650 try {
27651 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27652 gravityNorm, ba, new Matrix(1, 3));
27653 fail("IllegalArgumentException expected but not thrown");
27654 } catch (final IllegalArgumentException ignore) {
27655 }
27656 try {
27657 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27658 gravityNorm,
27659 ba, new Matrix(3, 1));
27660 fail("IllegalArgumentException expected but not thrown");
27661 } catch (final IllegalArgumentException ignore) {
27662 }
27663 assertNull(calibrator);
27664 }
27665
27666 @Test
27667 public void testConstructor228() throws WrongSizeException {
27668 final Matrix ba = generateBa();
27669 final double[] bias = ba.getBuffer();
27670 final double biasX = ba.getElementAtIndex(0);
27671 final double biasY = ba.getElementAtIndex(1);
27672 final double biasZ = ba.getElementAtIndex(2);
27673
27674 final Matrix ma = generateMaCommonAxis();
27675 final double sx = ma.getElementAt(0, 0);
27676 final double sy = ma.getElementAt(1, 1);
27677 final double sz = ma.getElementAt(2, 2);
27678 final double mxy = ma.getElementAt(0, 1);
27679 final double mxz = ma.getElementAt(0, 2);
27680 final double myx = ma.getElementAt(1, 0);
27681 final double myz = ma.getElementAt(1, 2);
27682 final double mzx = ma.getElementAt(2, 0);
27683 final double mzy = ma.getElementAt(2, 1);
27684
27685 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27686 final double latitude = Math.toRadians(
27687 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27688 final double longitude = Math.toRadians(
27689 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27690 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27691 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27692 final NEDVelocity nedVelocity = new NEDVelocity();
27693 final ECEFPosition ecefPosition = new ECEFPosition();
27694 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27695 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27696 ecefPosition, ecefVelocity);
27697 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27698 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27699 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27700
27701 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27702 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
27703 ba, ma, this);
27704
27705
27706 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27707 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27708 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27709 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27710 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27711 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27712 final Acceleration bx2 = new Acceleration(0.0,
27713 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27714 calibrator.getBiasXAsAcceleration(bx2);
27715 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27716 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27717 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27718 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27719 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27720 final Acceleration by2 = new Acceleration(0.0,
27721 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27722 calibrator.getBiasYAsAcceleration(by2);
27723 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27724 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27725 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27726 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27727 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27728 final Acceleration bz2 = new Acceleration(0.0,
27729 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27730 calibrator.getBiasZAsAcceleration(bz2);
27731 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27732 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27733 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27734 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27735 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27736 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27737 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27738 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27739 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27740 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27741 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27742 final double[] bias1 = calibrator.getBias();
27743 assertArrayEquals(bias1, bias, 0.0);
27744 final double[] bias2 = new double[3];
27745 calibrator.getBias(bias2);
27746 assertArrayEquals(bias1, bias2, 0.0);
27747 final Matrix b1 = calibrator.getBiasAsMatrix();
27748 assertEquals(b1, ba);
27749 final Matrix b2 = new Matrix(3, 1);
27750 calibrator.getBiasAsMatrix(b2);
27751 assertEquals(b1, b2);
27752 final Matrix ma1 = new Matrix(3, 3);
27753 ma1.setSubmatrix(0, 0,
27754 2, 2,
27755 new double[]{sx, myx, mzx,
27756 mxy, sy, mzy,
27757 mxz, myz, sz});
27758 assertEquals(calibrator.getInitialMa(), ma1);
27759 final Matrix ma2 = new Matrix(3, 3);
27760 calibrator.getInitialMa(ma2);
27761 assertEquals(ma1, ma2);
27762 assertNull(calibrator.getMeasurements());
27763 assertFalse(calibrator.isCommonAxisUsed());
27764 assertSame(calibrator.getListener(), this);
27765 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27766 assertFalse(calibrator.isReady());
27767 assertFalse(calibrator.isRunning());
27768 assertNull(calibrator.getEstimatedMa());
27769 assertNull(calibrator.getEstimatedSx());
27770 assertNull(calibrator.getEstimatedSy());
27771 assertNull(calibrator.getEstimatedSz());
27772 assertNull(calibrator.getEstimatedMxy());
27773 assertNull(calibrator.getEstimatedMxz());
27774 assertNull(calibrator.getEstimatedMyx());
27775 assertNull(calibrator.getEstimatedMyz());
27776 assertNull(calibrator.getEstimatedMzx());
27777 assertNull(calibrator.getEstimatedMzy());
27778 assertNull(calibrator.getEstimatedCovariance());
27779 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27780 assertNotNull(calibrator.getGroundTruthGravityNorm());
27781 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27782 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27783 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27784 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27785 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27786 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27787 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27788
27789
27790 final Acceleration invalidGravityNorm = new Acceleration(
27791 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27792
27793 calibrator = null;
27794 try {
27795 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27796 invalidGravityNorm, ba, ma, this);
27797 fail("IllegalArgumentException expected but not thrown");
27798 } catch (final IllegalArgumentException ignore) {
27799 }
27800 try {
27801 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27802 gravityNorm, new Matrix(1, 1), ma,
27803 this);
27804 fail("IllegalArgumentException expected but not thrown");
27805 } catch (final IllegalArgumentException ignore) {
27806 }
27807 try {
27808 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27809 gravityNorm, new Matrix(1, 3), ma,
27810 this);
27811 fail("IllegalArgumentException expected but not thrown");
27812 } catch (final IllegalArgumentException ignore) {
27813 }
27814 try {
27815 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27816 gravityNorm, ba, new Matrix(1, 3),
27817 this);
27818 fail("IllegalArgumentException expected but not thrown");
27819 } catch (final IllegalArgumentException ignore) {
27820 }
27821 try {
27822 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27823 gravityNorm, ba, new Matrix(3, 1),
27824 this);
27825 fail("IllegalArgumentException expected but not thrown");
27826 } catch (final IllegalArgumentException ignore) {
27827 }
27828 assertNull(calibrator);
27829 }
27830
27831 @Test
27832 public void testConstructor229() throws WrongSizeException {
27833 final Collection<StandardDeviationBodyKinematics> measurements =
27834 Collections.emptyList();
27835
27836 final Matrix ba = generateBa();
27837 final double[] bias = ba.getBuffer();
27838 final double biasX = ba.getElementAtIndex(0);
27839 final double biasY = ba.getElementAtIndex(1);
27840 final double biasZ = ba.getElementAtIndex(2);
27841
27842 final Matrix ma = generateMaCommonAxis();
27843 final double sx = ma.getElementAt(0, 0);
27844 final double sy = ma.getElementAt(1, 1);
27845 final double sz = ma.getElementAt(2, 2);
27846 final double mxy = ma.getElementAt(0, 1);
27847 final double mxz = ma.getElementAt(0, 2);
27848 final double myx = ma.getElementAt(1, 0);
27849 final double myz = ma.getElementAt(1, 2);
27850 final double mzx = ma.getElementAt(2, 0);
27851 final double mzy = ma.getElementAt(2, 1);
27852
27853 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27854 final double latitude = Math.toRadians(
27855 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27856 final double longitude = Math.toRadians(
27857 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27858 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27859 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27860 final NEDVelocity nedVelocity = new NEDVelocity();
27861 final ECEFPosition ecefPosition = new ECEFPosition();
27862 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27863 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27864 ecefPosition, ecefVelocity);
27865 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27866 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27867 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27868
27869 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27870 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
27871 measurements, ba, ma);
27872
27873
27874 assertEquals(calibrator.getBiasX(), biasX, 0.0);
27875 assertEquals(calibrator.getBiasY(), biasY, 0.0);
27876 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27877 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27878 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27879 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27880 final Acceleration bx2 = new Acceleration(0.0,
27881 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27882 calibrator.getBiasXAsAcceleration(bx2);
27883 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27884 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27885 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27886 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27887 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27888 final Acceleration by2 = new Acceleration(0.0,
27889 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27890 calibrator.getBiasYAsAcceleration(by2);
27891 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27892 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27893 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27894 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27895 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27896 final Acceleration bz2 = new Acceleration(0.0,
27897 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27898 calibrator.getBiasZAsAcceleration(bz2);
27899 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27900 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27901 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27902 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27903 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27904 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27905 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27906 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27907 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27908 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27909 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27910 final double[] bias1 = calibrator.getBias();
27911 assertArrayEquals(bias1, bias, 0.0);
27912 final double[] bias2 = new double[3];
27913 calibrator.getBias(bias2);
27914 assertArrayEquals(bias1, bias2, 0.0);
27915 final Matrix b1 = calibrator.getBiasAsMatrix();
27916 assertEquals(b1, ba);
27917 final Matrix b2 = new Matrix(3, 1);
27918 calibrator.getBiasAsMatrix(b2);
27919 assertEquals(b1, b2);
27920 final Matrix ma1 = new Matrix(3, 3);
27921 ma1.setSubmatrix(0, 0,
27922 2, 2,
27923 new double[]{sx, myx, mzx,
27924 mxy, sy, mzy,
27925 mxz, myz, sz});
27926 assertEquals(calibrator.getInitialMa(), ma1);
27927 final Matrix ma2 = new Matrix(3, 3);
27928 calibrator.getInitialMa(ma2);
27929 assertEquals(ma1, ma2);
27930 assertSame(calibrator.getMeasurements(), measurements);
27931 assertFalse(calibrator.isCommonAxisUsed());
27932 assertNull(calibrator.getListener());
27933 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27934 assertFalse(calibrator.isReady());
27935 assertFalse(calibrator.isRunning());
27936 assertNull(calibrator.getEstimatedMa());
27937 assertNull(calibrator.getEstimatedSx());
27938 assertNull(calibrator.getEstimatedSy());
27939 assertNull(calibrator.getEstimatedSz());
27940 assertNull(calibrator.getEstimatedMxy());
27941 assertNull(calibrator.getEstimatedMxz());
27942 assertNull(calibrator.getEstimatedMyx());
27943 assertNull(calibrator.getEstimatedMyz());
27944 assertNull(calibrator.getEstimatedMzx());
27945 assertNull(calibrator.getEstimatedMzy());
27946 assertNull(calibrator.getEstimatedCovariance());
27947 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27948 assertNotNull(calibrator.getGroundTruthGravityNorm());
27949 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27950 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27951 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27952 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27953 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27954 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27955 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27956
27957
27958 final Acceleration invalidGravityNorm = new Acceleration(
27959 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27960
27961 calibrator = null;
27962 try {
27963 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27964 invalidGravityNorm, ba, ma, this);
27965 fail("IllegalArgumentException expected but not thrown");
27966 } catch (final IllegalArgumentException ignore) {
27967 }
27968 try {
27969 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27970 gravityNorm, measurements,
27971 new Matrix(1, 1), ma);
27972 fail("IllegalArgumentException expected but not thrown");
27973 } catch (final IllegalArgumentException ignore) {
27974 }
27975 try {
27976 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27977 gravityNorm, measurements,
27978 new Matrix(1, 3), ma);
27979 fail("IllegalArgumentException expected but not thrown");
27980 } catch (final IllegalArgumentException ignore) {
27981 }
27982 try {
27983 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27984 gravityNorm, measurements, ba,
27985 new Matrix(1, 3));
27986 fail("IllegalArgumentException expected but not thrown");
27987 } catch (final IllegalArgumentException ignore) {
27988 }
27989 try {
27990 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27991 gravityNorm, measurements, ba,
27992 new Matrix(3, 1));
27993 fail("IllegalArgumentException expected but not thrown");
27994 } catch (final IllegalArgumentException ignore) {
27995 }
27996 assertNull(calibrator);
27997 }
27998
27999 @Test
28000 public void testConstructor230() throws WrongSizeException {
28001 final Collection<StandardDeviationBodyKinematics> measurements =
28002 Collections.emptyList();
28003
28004 final Matrix ba = generateBa();
28005 final double[] bias = ba.getBuffer();
28006 final double biasX = ba.getElementAtIndex(0);
28007 final double biasY = ba.getElementAtIndex(1);
28008 final double biasZ = ba.getElementAtIndex(2);
28009
28010 final Matrix ma = generateMaCommonAxis();
28011 final double sx = ma.getElementAt(0, 0);
28012 final double sy = ma.getElementAt(1, 1);
28013 final double sz = ma.getElementAt(2, 2);
28014 final double mxy = ma.getElementAt(0, 1);
28015 final double mxz = ma.getElementAt(0, 2);
28016 final double myx = ma.getElementAt(1, 0);
28017 final double myz = ma.getElementAt(1, 2);
28018 final double mzx = ma.getElementAt(2, 0);
28019 final double mzy = ma.getElementAt(2, 1);
28020
28021 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28022 final double latitude = Math.toRadians(
28023 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28024 final double longitude = Math.toRadians(
28025 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28026 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28027 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28028 final NEDVelocity nedVelocity = new NEDVelocity();
28029 final ECEFPosition ecefPosition = new ECEFPosition();
28030 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28031 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28032 ecefPosition, ecefVelocity);
28033 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28034 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28035 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28036
28037 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28038 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28039 measurements, ba, ma, this);
28040
28041
28042 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28043 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28044 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28045 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28046 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28047 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28048 final Acceleration bx2 = new Acceleration(0.0,
28049 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28050 calibrator.getBiasXAsAcceleration(bx2);
28051 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28052 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28053 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28054 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28055 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28056 final Acceleration by2 = new Acceleration(0.0,
28057 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28058 calibrator.getBiasYAsAcceleration(by2);
28059 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28060 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28061 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28062 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28063 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28064 final Acceleration bz2 = new Acceleration(0.0,
28065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28066 calibrator.getBiasZAsAcceleration(bz2);
28067 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28068 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28069 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28070 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28071 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28072 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28073 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28074 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28075 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28076 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28077 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28078 final double[] bias1 = calibrator.getBias();
28079 assertArrayEquals(bias1, bias, 0.0);
28080 final double[] bias2 = new double[3];
28081 calibrator.getBias(bias2);
28082 assertArrayEquals(bias1, bias2, 0.0);
28083 final Matrix b1 = calibrator.getBiasAsMatrix();
28084 assertEquals(b1, ba);
28085 final Matrix b2 = new Matrix(3, 1);
28086 calibrator.getBiasAsMatrix(b2);
28087 assertEquals(b1, b2);
28088 final Matrix ma1 = new Matrix(3, 3);
28089 ma1.setSubmatrix(0, 0,
28090 2, 2,
28091 new double[]{sx, myx, mzx,
28092 mxy, sy, mzy,
28093 mxz, myz, sz});
28094 assertEquals(calibrator.getInitialMa(), ma1);
28095 final Matrix ma2 = new Matrix(3, 3);
28096 calibrator.getInitialMa(ma2);
28097 assertEquals(ma1, ma2);
28098 assertSame(calibrator.getMeasurements(), measurements);
28099 assertFalse(calibrator.isCommonAxisUsed());
28100 assertSame(calibrator.getListener(), this);
28101 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28102 assertFalse(calibrator.isReady());
28103 assertFalse(calibrator.isRunning());
28104 assertNull(calibrator.getEstimatedMa());
28105 assertNull(calibrator.getEstimatedSx());
28106 assertNull(calibrator.getEstimatedSy());
28107 assertNull(calibrator.getEstimatedSz());
28108 assertNull(calibrator.getEstimatedMxy());
28109 assertNull(calibrator.getEstimatedMxz());
28110 assertNull(calibrator.getEstimatedMyx());
28111 assertNull(calibrator.getEstimatedMyz());
28112 assertNull(calibrator.getEstimatedMzx());
28113 assertNull(calibrator.getEstimatedMzy());
28114 assertNull(calibrator.getEstimatedCovariance());
28115 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28116 assertNotNull(calibrator.getGroundTruthGravityNorm());
28117 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28118 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28119 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28120 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28121 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28122 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28123 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28124
28125
28126 final Acceleration invalidGravityNorm = new Acceleration(
28127 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28128
28129 calibrator = null;
28130 try {
28131 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28132 invalidGravityNorm, ba, ma, this);
28133 fail("IllegalArgumentException expected but not thrown");
28134 } catch (final IllegalArgumentException ignore) {
28135 }
28136 try {
28137 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28138 gravityNorm, measurements,
28139 new Matrix(1, 1), ma, this);
28140 fail("IllegalArgumentException expected but not thrown");
28141 } catch (final IllegalArgumentException ignore) {
28142 }
28143 try {
28144 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28145 gravityNorm, measurements,
28146 new Matrix(1, 3), ma, this);
28147 fail("IllegalArgumentException expected but not thrown");
28148 } catch (final IllegalArgumentException ignore) {
28149 }
28150 try {
28151 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28152 gravityNorm, measurements, ba,
28153 new Matrix(1, 3), this);
28154 fail("IllegalArgumentException expected but not thrown");
28155 } catch (final IllegalArgumentException ignore) {
28156 }
28157 try {
28158 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28159 gravityNorm, measurements, ba,
28160 new Matrix(3, 1), this);
28161 fail("IllegalArgumentException expected but not thrown");
28162 } catch (final IllegalArgumentException ignore) {
28163 }
28164 assertNull(calibrator);
28165 }
28166
28167 @Test
28168 public void testConstructor231() throws WrongSizeException {
28169 final Matrix ba = generateBa();
28170 final double[] bias = ba.getBuffer();
28171 final double biasX = ba.getElementAtIndex(0);
28172 final double biasY = ba.getElementAtIndex(1);
28173 final double biasZ = ba.getElementAtIndex(2);
28174
28175 final Matrix ma = generateMaCommonAxis();
28176 final double sx = ma.getElementAt(0, 0);
28177 final double sy = ma.getElementAt(1, 1);
28178 final double sz = ma.getElementAt(2, 2);
28179 final double mxy = ma.getElementAt(0, 1);
28180 final double mxz = ma.getElementAt(0, 2);
28181 final double myx = ma.getElementAt(1, 0);
28182 final double myz = ma.getElementAt(1, 2);
28183 final double mzx = ma.getElementAt(2, 0);
28184 final double mzy = ma.getElementAt(2, 1);
28185
28186 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28187 final double latitude = Math.toRadians(
28188 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28189 final double longitude = Math.toRadians(
28190 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28191 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28192 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28193 final NEDVelocity nedVelocity = new NEDVelocity();
28194 final ECEFPosition ecefPosition = new ECEFPosition();
28195 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28196 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28197 ecefPosition, ecefVelocity);
28198 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28199 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28200 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28201
28202 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28203 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28204 true, ba, ma);
28205
28206
28207 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28208 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28209 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28210 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28211 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28212 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28213 final Acceleration bx2 = new Acceleration(0.0,
28214 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28215 calibrator.getBiasXAsAcceleration(bx2);
28216 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28217 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28218 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28219 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28220 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28221 final Acceleration by2 = new Acceleration(0.0,
28222 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28223 calibrator.getBiasYAsAcceleration(by2);
28224 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28225 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28226 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28227 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28228 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28229 final Acceleration bz2 = new Acceleration(0.0,
28230 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28231 calibrator.getBiasZAsAcceleration(bz2);
28232 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28233 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28234 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28235 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28236 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28237 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28238 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28239 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28240 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28241 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28242 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28243 final double[] bias1 = calibrator.getBias();
28244 assertArrayEquals(bias1, bias, 0.0);
28245 final double[] bias2 = new double[3];
28246 calibrator.getBias(bias2);
28247 assertArrayEquals(bias1, bias2, 0.0);
28248 final Matrix b1 = calibrator.getBiasAsMatrix();
28249 assertEquals(b1, ba);
28250 final Matrix b2 = new Matrix(3, 1);
28251 calibrator.getBiasAsMatrix(b2);
28252 assertEquals(b1, b2);
28253 final Matrix ma1 = new Matrix(3, 3);
28254 ma1.setSubmatrix(0, 0,
28255 2, 2,
28256 new double[]{sx, myx, mzx,
28257 mxy, sy, mzy,
28258 mxz, myz, sz});
28259 assertEquals(calibrator.getInitialMa(), ma1);
28260 final Matrix ma2 = new Matrix(3, 3);
28261 calibrator.getInitialMa(ma2);
28262 assertEquals(ma1, ma2);
28263 assertNull(calibrator.getMeasurements());
28264 assertTrue(calibrator.isCommonAxisUsed());
28265 assertNull(calibrator.getListener());
28266 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28267 assertFalse(calibrator.isReady());
28268 assertFalse(calibrator.isRunning());
28269 assertNull(calibrator.getEstimatedMa());
28270 assertNull(calibrator.getEstimatedSx());
28271 assertNull(calibrator.getEstimatedSy());
28272 assertNull(calibrator.getEstimatedSz());
28273 assertNull(calibrator.getEstimatedMxy());
28274 assertNull(calibrator.getEstimatedMxz());
28275 assertNull(calibrator.getEstimatedMyx());
28276 assertNull(calibrator.getEstimatedMyz());
28277 assertNull(calibrator.getEstimatedMzx());
28278 assertNull(calibrator.getEstimatedMzy());
28279 assertNull(calibrator.getEstimatedCovariance());
28280 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28281 assertNotNull(calibrator.getGroundTruthGravityNorm());
28282 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28283 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28284 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28285 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28286 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28287 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28288 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28289
28290
28291 final Acceleration invalidGravityNorm = new Acceleration(
28292 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28293
28294 calibrator = null;
28295 try {
28296 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28297 invalidGravityNorm, true, ba, ma);
28298 fail("IllegalArgumentException expected but not thrown");
28299 } catch (final IllegalArgumentException ignore) {
28300 }
28301 try {
28302 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28303 gravityNorm, true,
28304 new Matrix(1, 1), ma);
28305 fail("IllegalArgumentException expected but not thrown");
28306 } catch (final IllegalArgumentException ignore) {
28307 }
28308 try {
28309 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28310 gravityNorm, true,
28311 new Matrix(1, 3), ma);
28312 fail("IllegalArgumentException expected but not thrown");
28313 } catch (final IllegalArgumentException ignore) {
28314 }
28315 try {
28316 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28317 gravityNorm, true, ba,
28318 new Matrix(1, 3));
28319 fail("IllegalArgumentException expected but not thrown");
28320 } catch (final IllegalArgumentException ignore) {
28321 }
28322 try {
28323 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28324 gravityNorm, true, ba,
28325 new Matrix(3, 1));
28326 fail("IllegalArgumentException expected but not thrown");
28327 } catch (final IllegalArgumentException ignore) {
28328 }
28329 assertNull(calibrator);
28330 }
28331
28332 @Test
28333 public void testConstructor232() throws WrongSizeException {
28334 final Matrix ba = generateBa();
28335 final double[] bias = ba.getBuffer();
28336 final double biasX = ba.getElementAtIndex(0);
28337 final double biasY = ba.getElementAtIndex(1);
28338 final double biasZ = ba.getElementAtIndex(2);
28339
28340 final Matrix ma = generateMaCommonAxis();
28341 final double sx = ma.getElementAt(0, 0);
28342 final double sy = ma.getElementAt(1, 1);
28343 final double sz = ma.getElementAt(2, 2);
28344 final double mxy = ma.getElementAt(0, 1);
28345 final double mxz = ma.getElementAt(0, 2);
28346 final double myx = ma.getElementAt(1, 0);
28347 final double myz = ma.getElementAt(1, 2);
28348 final double mzx = ma.getElementAt(2, 0);
28349 final double mzy = ma.getElementAt(2, 1);
28350
28351 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28352 final double latitude = Math.toRadians(
28353 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28354 final double longitude = Math.toRadians(
28355 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28356 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28357 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28358 final NEDVelocity nedVelocity = new NEDVelocity();
28359 final ECEFPosition ecefPosition = new ECEFPosition();
28360 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28361 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28362 ecefPosition, ecefVelocity);
28363 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28364 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28365 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28366
28367 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28368 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28369 true, ba, ma, this);
28370
28371
28372 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28373 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28374 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28375 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28376 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28377 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28378 final Acceleration bx2 = new Acceleration(0.0,
28379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28380 calibrator.getBiasXAsAcceleration(bx2);
28381 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28382 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28383 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28384 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28385 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28386 final Acceleration by2 = new Acceleration(0.0,
28387 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28388 calibrator.getBiasYAsAcceleration(by2);
28389 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28390 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28391 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28392 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28393 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28394 final Acceleration bz2 = new Acceleration(0.0,
28395 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28396 calibrator.getBiasZAsAcceleration(bz2);
28397 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28398 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28399 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28400 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28401 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28402 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28403 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28404 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28405 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28406 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28407 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28408 final double[] bias1 = calibrator.getBias();
28409 assertArrayEquals(bias1, bias, 0.0);
28410 final double[] bias2 = new double[3];
28411 calibrator.getBias(bias2);
28412 assertArrayEquals(bias1, bias2, 0.0);
28413 final Matrix b1 = calibrator.getBiasAsMatrix();
28414 assertEquals(b1, ba);
28415 final Matrix b2 = new Matrix(3, 1);
28416 calibrator.getBiasAsMatrix(b2);
28417 assertEquals(b1, b2);
28418 final Matrix ma1 = new Matrix(3, 3);
28419 ma1.setSubmatrix(0, 0,
28420 2, 2,
28421 new double[]{sx, myx, mzx,
28422 mxy, sy, mzy,
28423 mxz, myz, sz});
28424 assertEquals(calibrator.getInitialMa(), ma1);
28425 final Matrix ma2 = new Matrix(3, 3);
28426 calibrator.getInitialMa(ma2);
28427 assertEquals(ma1, ma2);
28428 assertNull(calibrator.getMeasurements());
28429 assertTrue(calibrator.isCommonAxisUsed());
28430 assertSame(calibrator.getListener(), this);
28431 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28432 assertFalse(calibrator.isReady());
28433 assertFalse(calibrator.isRunning());
28434 assertNull(calibrator.getEstimatedMa());
28435 assertNull(calibrator.getEstimatedSx());
28436 assertNull(calibrator.getEstimatedSy());
28437 assertNull(calibrator.getEstimatedSz());
28438 assertNull(calibrator.getEstimatedMxy());
28439 assertNull(calibrator.getEstimatedMxz());
28440 assertNull(calibrator.getEstimatedMyx());
28441 assertNull(calibrator.getEstimatedMyz());
28442 assertNull(calibrator.getEstimatedMzx());
28443 assertNull(calibrator.getEstimatedMzy());
28444 assertNull(calibrator.getEstimatedCovariance());
28445 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28446 assertNotNull(calibrator.getGroundTruthGravityNorm());
28447 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28448 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28449 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28450 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28451 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28452 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28453 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28454
28455
28456 final Acceleration invalidGravityNorm = new Acceleration(
28457 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28458
28459 calibrator = null;
28460 try {
28461 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28462 invalidGravityNorm, true, ba, ma);
28463 fail("IllegalArgumentException expected but not thrown");
28464 } catch (final IllegalArgumentException ignore) {
28465 }
28466 try {
28467 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28468 gravityNorm, true,
28469 new Matrix(1, 1), ma, this);
28470 fail("IllegalArgumentException expected but not thrown");
28471 } catch (final IllegalArgumentException ignore) {
28472 }
28473 try {
28474 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28475 gravityNorm, true,
28476 new Matrix(1, 3), ma, this);
28477 fail("IllegalArgumentException expected but not thrown");
28478 } catch (final IllegalArgumentException ignore) {
28479 }
28480 try {
28481 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28482 gravityNorm, true, ba,
28483 new Matrix(1, 3), this);
28484 fail("IllegalArgumentException expected but not thrown");
28485 } catch (final IllegalArgumentException ignore) {
28486 }
28487 try {
28488 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28489 gravityNorm, true, ba,
28490 new Matrix(3, 1), this);
28491 fail("IllegalArgumentException expected but not thrown");
28492 } catch (final IllegalArgumentException ignore) {
28493 }
28494 assertNull(calibrator);
28495 }
28496
28497 @Test
28498 public void testConstructor233() throws WrongSizeException {
28499 final Collection<StandardDeviationBodyKinematics> measurements =
28500 Collections.emptyList();
28501
28502 final Matrix ba = generateBa();
28503 final double[] bias = ba.getBuffer();
28504 final double biasX = ba.getElementAtIndex(0);
28505 final double biasY = ba.getElementAtIndex(1);
28506 final double biasZ = ba.getElementAtIndex(2);
28507
28508 final Matrix ma = generateMaCommonAxis();
28509 final double sx = ma.getElementAt(0, 0);
28510 final double sy = ma.getElementAt(1, 1);
28511 final double sz = ma.getElementAt(2, 2);
28512 final double mxy = ma.getElementAt(0, 1);
28513 final double mxz = ma.getElementAt(0, 2);
28514 final double myx = ma.getElementAt(1, 0);
28515 final double myz = ma.getElementAt(1, 2);
28516 final double mzx = ma.getElementAt(2, 0);
28517 final double mzy = ma.getElementAt(2, 1);
28518
28519 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28520 final double latitude = Math.toRadians(
28521 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28522 final double longitude = Math.toRadians(
28523 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28524 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28525 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28526 final NEDVelocity nedVelocity = new NEDVelocity();
28527 final ECEFPosition ecefPosition = new ECEFPosition();
28528 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28529 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28530 ecefPosition, ecefVelocity);
28531 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28532 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28533 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28534
28535 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28536 new KnownBiasAndGravityNormAccelerometerCalibrator(
28537 gravityNorm, measurements, true,
28538 ba, ma);
28539
28540
28541 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28542 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28543 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28544 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28545 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28547 final Acceleration bx2 = new Acceleration(0.0,
28548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28549 calibrator.getBiasXAsAcceleration(bx2);
28550 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28552 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28553 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28555 final Acceleration by2 = new Acceleration(0.0,
28556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28557 calibrator.getBiasYAsAcceleration(by2);
28558 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28560 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28561 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28563 final Acceleration bz2 = new Acceleration(0.0,
28564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28565 calibrator.getBiasZAsAcceleration(bz2);
28566 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28568 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28569 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28570 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28571 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28572 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28573 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28574 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28575 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28576 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28577 final double[] bias1 = calibrator.getBias();
28578 assertArrayEquals(bias1, bias, 0.0);
28579 final double[] bias2 = new double[3];
28580 calibrator.getBias(bias2);
28581 assertArrayEquals(bias1, bias2, 0.0);
28582 final Matrix b1 = calibrator.getBiasAsMatrix();
28583 assertEquals(b1, ba);
28584 final Matrix b2 = new Matrix(3, 1);
28585 calibrator.getBiasAsMatrix(b2);
28586 assertEquals(b1, b2);
28587 final Matrix ma1 = new Matrix(3, 3);
28588 ma1.setSubmatrix(0, 0,
28589 2, 2,
28590 new double[]{sx, myx, mzx,
28591 mxy, sy, mzy,
28592 mxz, myz, sz});
28593 assertEquals(calibrator.getInitialMa(), ma1);
28594 final Matrix ma2 = new Matrix(3, 3);
28595 calibrator.getInitialMa(ma2);
28596 assertEquals(ma1, ma2);
28597 assertSame(calibrator.getMeasurements(), measurements);
28598 assertTrue(calibrator.isCommonAxisUsed());
28599 assertNull(calibrator.getListener());
28600 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28601 assertFalse(calibrator.isReady());
28602 assertFalse(calibrator.isRunning());
28603 assertNull(calibrator.getEstimatedMa());
28604 assertNull(calibrator.getEstimatedSx());
28605 assertNull(calibrator.getEstimatedSy());
28606 assertNull(calibrator.getEstimatedSz());
28607 assertNull(calibrator.getEstimatedMxy());
28608 assertNull(calibrator.getEstimatedMxz());
28609 assertNull(calibrator.getEstimatedMyx());
28610 assertNull(calibrator.getEstimatedMyz());
28611 assertNull(calibrator.getEstimatedMzx());
28612 assertNull(calibrator.getEstimatedMzy());
28613 assertNull(calibrator.getEstimatedCovariance());
28614 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28615 assertNotNull(calibrator.getGroundTruthGravityNorm());
28616 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28617 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28618 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28619 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28620 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28621 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28622 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28623
28624
28625 final Acceleration invalidGravityNorm = new Acceleration(
28626 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28627
28628 calibrator = null;
28629 try {
28630 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28631 invalidGravityNorm, measurements, true,
28632 ba, ma);
28633 fail("IllegalArgumentException expected but not thrown");
28634 } catch (final IllegalArgumentException ignore) {
28635 }
28636 try {
28637 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28638 gravityNorm, measurements, true,
28639 new Matrix(1, 1), ma);
28640 fail("IllegalArgumentException expected but not thrown");
28641 } catch (final IllegalArgumentException ignore) {
28642 }
28643 try {
28644 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28645 gravityNorm, measurements, true,
28646 new Matrix(1, 3), ma);
28647 fail("IllegalArgumentException expected but not thrown");
28648 } catch (final IllegalArgumentException ignore) {
28649 }
28650 try {
28651 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28652 gravityNorm, measurements, true,
28653 ba, new Matrix(1, 3));
28654 fail("IllegalArgumentException expected but not thrown");
28655 } catch (final IllegalArgumentException ignore) {
28656 }
28657 try {
28658 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28659 gravityNorm, measurements, true,
28660 ba, new Matrix(3, 1));
28661 fail("IllegalArgumentException expected but not thrown");
28662 } catch (final IllegalArgumentException ignore) {
28663 }
28664 assertNull(calibrator);
28665 }
28666
28667 @Test
28668 public void testConstructor234() throws WrongSizeException {
28669 final Collection<StandardDeviationBodyKinematics> measurements =
28670 Collections.emptyList();
28671
28672 final Matrix ba = generateBa();
28673 final double[] bias = ba.getBuffer();
28674 final double biasX = ba.getElementAtIndex(0);
28675 final double biasY = ba.getElementAtIndex(1);
28676 final double biasZ = ba.getElementAtIndex(2);
28677
28678 final Matrix ma = generateMaCommonAxis();
28679 final double sx = ma.getElementAt(0, 0);
28680 final double sy = ma.getElementAt(1, 1);
28681 final double sz = ma.getElementAt(2, 2);
28682 final double mxy = ma.getElementAt(0, 1);
28683 final double mxz = ma.getElementAt(0, 2);
28684 final double myx = ma.getElementAt(1, 0);
28685 final double myz = ma.getElementAt(1, 2);
28686 final double mzx = ma.getElementAt(2, 0);
28687 final double mzy = ma.getElementAt(2, 1);
28688
28689 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28690 final double latitude = Math.toRadians(
28691 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28692 final double longitude = Math.toRadians(
28693 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28694 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28695 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28696 final NEDVelocity nedVelocity = new NEDVelocity();
28697 final ECEFPosition ecefPosition = new ECEFPosition();
28698 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28699 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28700 ecefPosition, ecefVelocity);
28701 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28702 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28703 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28704
28705 KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28706 new KnownBiasAndGravityNormAccelerometerCalibrator(
28707 gravityNorm, measurements,
28708 true, ba, ma, this);
28709
28710
28711 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28712 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28713 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28714 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28715 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28716 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28717 final Acceleration bx2 = new Acceleration(0.0,
28718 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28719 calibrator.getBiasXAsAcceleration(bx2);
28720 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28721 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28722 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28723 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28724 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28725 final Acceleration by2 = new Acceleration(0.0,
28726 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28727 calibrator.getBiasYAsAcceleration(by2);
28728 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28729 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28730 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28731 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28732 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28733 final Acceleration bz2 = new Acceleration(0.0,
28734 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28735 calibrator.getBiasZAsAcceleration(bz2);
28736 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28737 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28738 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28739 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28740 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28741 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28742 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28743 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28744 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28745 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28746 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28747 final double[] bias1 = calibrator.getBias();
28748 assertArrayEquals(bias1, bias, 0.0);
28749 final double[] bias2 = new double[3];
28750 calibrator.getBias(bias2);
28751 assertArrayEquals(bias1, bias2, 0.0);
28752 final Matrix b1 = calibrator.getBiasAsMatrix();
28753 assertEquals(b1, ba);
28754 final Matrix b2 = new Matrix(3, 1);
28755 calibrator.getBiasAsMatrix(b2);
28756 assertEquals(b1, b2);
28757 final Matrix ma1 = new Matrix(3, 3);
28758 ma1.setSubmatrix(0, 0,
28759 2, 2,
28760 new double[]{sx, myx, mzx,
28761 mxy, sy, mzy,
28762 mxz, myz, sz});
28763 assertEquals(calibrator.getInitialMa(), ma1);
28764 final Matrix ma2 = new Matrix(3, 3);
28765 calibrator.getInitialMa(ma2);
28766 assertEquals(ma1, ma2);
28767 assertSame(calibrator.getMeasurements(), measurements);
28768 assertTrue(calibrator.isCommonAxisUsed());
28769 assertSame(calibrator.getListener(), this);
28770 assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28771 assertFalse(calibrator.isReady());
28772 assertFalse(calibrator.isRunning());
28773 assertNull(calibrator.getEstimatedMa());
28774 assertNull(calibrator.getEstimatedSx());
28775 assertNull(calibrator.getEstimatedSy());
28776 assertNull(calibrator.getEstimatedSz());
28777 assertNull(calibrator.getEstimatedMxy());
28778 assertNull(calibrator.getEstimatedMxz());
28779 assertNull(calibrator.getEstimatedMyx());
28780 assertNull(calibrator.getEstimatedMyz());
28781 assertNull(calibrator.getEstimatedMzx());
28782 assertNull(calibrator.getEstimatedMzy());
28783 assertNull(calibrator.getEstimatedCovariance());
28784 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28785 assertNotNull(calibrator.getGroundTruthGravityNorm());
28786 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28787 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28788 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28789 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28790 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28791 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28792 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28793
28794
28795 final Acceleration invalidGravityNorm = new Acceleration(
28796 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28797
28798 calibrator = null;
28799 try {
28800 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28801 invalidGravityNorm, measurements,
28802 true, ba, ma, this);
28803 fail("IllegalArgumentException expected but not thrown");
28804 } catch (final IllegalArgumentException ignore) {
28805 }
28806 try {
28807 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28808 gravityNorm, measurements, true,
28809 new Matrix(1, 1), ma, this);
28810 fail("IllegalArgumentException expected but not thrown");
28811 } catch (final IllegalArgumentException ignore) {
28812 }
28813 try {
28814 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28815 gravityNorm, measurements, true,
28816 new Matrix(1, 3), ma, this);
28817 fail("IllegalArgumentException expected but not thrown");
28818 } catch (final IllegalArgumentException ignore) {
28819 }
28820 try {
28821 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28822 gravityNorm, measurements, true,
28823 ba, new Matrix(1, 3), this);
28824 fail("IllegalArgumentException expected but not thrown");
28825 } catch (final IllegalArgumentException ignore) {
28826 }
28827 try {
28828 calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28829 gravityNorm, measurements, true,
28830 ba, new Matrix(3, 1), this);
28831 fail("IllegalArgumentException expected but not thrown");
28832 } catch (final IllegalArgumentException ignore) {
28833 }
28834 assertNull(calibrator);
28835 }
28836
28837 @Test
28838 public void testGetSetBiasX() throws LockedException {
28839 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28840 new KnownBiasAndGravityNormAccelerometerCalibrator();
28841
28842 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
28843
28844
28845 final Matrix ba = generateBa();
28846 final double biasX = ba.getElementAtIndex(0);
28847
28848 calibrator.setBiasX(biasX);
28849
28850
28851 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28852 }
28853
28854 @Test
28855 public void testGetSetBiasY() throws LockedException {
28856 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28857 new KnownBiasAndGravityNormAccelerometerCalibrator();
28858
28859 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
28860
28861
28862 final Matrix ba = generateBa();
28863 final double biasY = ba.getElementAtIndex(1);
28864
28865 calibrator.setBiasY(biasY);
28866
28867
28868 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28869 }
28870
28871 @Test
28872 public void testGetSetBiasZ() throws LockedException {
28873 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28874 new KnownBiasAndGravityNormAccelerometerCalibrator();
28875
28876 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
28877
28878
28879 final Matrix ba = generateBa();
28880 final double biasZ = ba.getElementAtIndex(2);
28881
28882 calibrator.setBiasZ(biasZ);
28883
28884
28885 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28886 }
28887
28888 @Test
28889 public void testGetSetBiasXAsAcceleration() throws LockedException {
28890 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28891 new KnownBiasAndGravityNormAccelerometerCalibrator();
28892
28893
28894 final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28895
28896 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
28897 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28898
28899
28900 final Matrix ba = generateBa();
28901 final double biasX = ba.getElementAtIndex(0);
28902
28903 final Acceleration bx2 = new Acceleration(biasX,
28904 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28905 calibrator.setBiasX(bx2);
28906
28907
28908 final Acceleration bx3 = calibrator.getBiasXAsAcceleration();
28909 final Acceleration bx4 = new Acceleration(biasX,
28910 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28911 calibrator.getBiasXAsAcceleration(bx4);
28912
28913 assertEquals(bx3, bx4);
28914 }
28915
28916 @Test
28917 public void testGetSetBiasYAsAcceleration() throws LockedException {
28918 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28919 new KnownBiasAndGravityNormAccelerometerCalibrator();
28920
28921
28922 final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28923
28924 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
28925 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28926
28927
28928 final Matrix ba = generateBa();
28929 final double biasY = ba.getElementAtIndex(1);
28930
28931 final Acceleration by2 = new Acceleration(biasY,
28932 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28933 calibrator.setBiasY(by2);
28934
28935
28936 final Acceleration by3 = calibrator.getBiasYAsAcceleration();
28937 final Acceleration by4 = new Acceleration(biasY,
28938 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28939 calibrator.getBiasYAsAcceleration(by4);
28940
28941 assertEquals(by3, by4);
28942 }
28943
28944 @Test
28945 public void testGetSetBiasZAsAcceleration() throws LockedException {
28946 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28947 new KnownBiasAndGravityNormAccelerometerCalibrator();
28948
28949
28950 final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28951
28952 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
28953 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28954
28955
28956 final Matrix ba = generateBa();
28957 final double biasZ = ba.getElementAtIndex(2);
28958
28959 final Acceleration bz2 = new Acceleration(biasZ,
28960 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28961 calibrator.setBiasZ(bz2);
28962
28963
28964 final Acceleration bz3 = calibrator.getBiasZAsAcceleration();
28965 final Acceleration bz4 = new Acceleration(biasZ,
28966 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28967 calibrator.getBiasZAsAcceleration(bz4);
28968
28969 assertEquals(bz3, bz4);
28970 }
28971
28972 @Test
28973 public void testSetBias1() throws LockedException {
28974 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28975 new KnownBiasAndGravityNormAccelerometerCalibrator();
28976
28977
28978 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
28979 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
28980 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
28981
28982
28983 final Matrix ba = generateBa();
28984 final double biasX = ba.getElementAtIndex(0);
28985 final double biasY = ba.getElementAtIndex(1);
28986 final double biasZ = ba.getElementAtIndex(2);
28987
28988 calibrator.setBias(biasX, biasY, biasZ);
28989
28990
28991 assertEquals(calibrator.getBiasX(), biasX, 0.0);
28992 assertEquals(calibrator.getBiasY(), biasY, 0.0);
28993 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28994 }
28995
28996 @Test
28997 public void testSetBias2() throws LockedException {
28998 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28999 new KnownBiasAndGravityNormAccelerometerCalibrator();
29000
29001
29002 assertEquals(calibrator.getBiasX(), 0.0, 0.0);
29003 assertEquals(calibrator.getBiasY(), 0.0, 0.0);
29004 assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
29005
29006
29007 final Matrix ba = generateBa();
29008 final double biasX = ba.getElementAtIndex(0);
29009 final double biasY = ba.getElementAtIndex(1);
29010 final double biasZ = ba.getElementAtIndex(2);
29011
29012 final Acceleration bx = new Acceleration(biasX,
29013 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29014 final Acceleration by = new Acceleration(biasY,
29015 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29016 final Acceleration bz = new Acceleration(biasZ,
29017 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29018
29019 calibrator.setBias(bx, by, bz);
29020
29021
29022 assertEquals(calibrator.getBiasX(), biasX, 0.0);
29023 assertEquals(calibrator.getBiasY(), biasY, 0.0);
29024 assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
29025 }
29026
29027 @Test
29028 public void testGetSetInitialSx() throws WrongSizeException, LockedException {
29029 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29030 new KnownBiasAndGravityNormAccelerometerCalibrator();
29031
29032 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29033
29034
29035 final Matrix ma = generateMaGeneral();
29036 final double sx = ma.getElementAt(0, 0);
29037
29038 calibrator.setInitialSx(sx);
29039
29040
29041 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29042 }
29043
29044 @Test
29045 public void testGetSetInitialSy() throws WrongSizeException, LockedException {
29046 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29047 new KnownBiasAndGravityNormAccelerometerCalibrator();
29048
29049 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29050
29051
29052 final Matrix ma = generateMaGeneral();
29053 final double sy = ma.getElementAt(1, 1);
29054
29055 calibrator.setInitialSy(sy);
29056
29057
29058 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29059 }
29060
29061 @Test
29062 public void testGetSetInitialSz() throws WrongSizeException, LockedException {
29063 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29064 new KnownBiasAndGravityNormAccelerometerCalibrator();
29065
29066 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29067
29068
29069 final Matrix ma = generateMaGeneral();
29070 final double sz = ma.getElementAt(2, 2);
29071
29072 calibrator.setInitialSz(sz);
29073
29074
29075 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29076 }
29077
29078 @Test
29079 public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
29080 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29081 new KnownBiasAndGravityNormAccelerometerCalibrator();
29082
29083 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29084
29085
29086 final Matrix ma = generateMaGeneral();
29087 final double mxy = ma.getElementAt(0, 1);
29088
29089 calibrator.setInitialMxy(mxy);
29090
29091
29092 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29093 }
29094
29095 @Test
29096 public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
29097 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29098 new KnownBiasAndGravityNormAccelerometerCalibrator();
29099
29100 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29101
29102
29103 final Matrix ma = generateMaGeneral();
29104 final double mxz = ma.getElementAt(0, 2);
29105
29106 calibrator.setInitialMxz(mxz);
29107
29108
29109 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29110 }
29111
29112 @Test
29113 public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
29114 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29115 new KnownBiasAndGravityNormAccelerometerCalibrator();
29116
29117 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29118
29119
29120 final Matrix ma = generateMaGeneral();
29121 final double myx = ma.getElementAt(1, 0);
29122
29123 calibrator.setInitialMyx(myx);
29124
29125
29126 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29127 }
29128
29129 @Test
29130 public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
29131 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29132 new KnownBiasAndGravityNormAccelerometerCalibrator();
29133
29134 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29135
29136
29137 final Matrix ma = generateMaGeneral();
29138 final double myz = ma.getElementAt(1, 2);
29139
29140 calibrator.setInitialMyz(myz);
29141
29142
29143 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29144 }
29145
29146 @Test
29147 public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
29148 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29149 new KnownBiasAndGravityNormAccelerometerCalibrator();
29150
29151 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29152
29153
29154 final Matrix ma = generateMaGeneral();
29155 final double mzx = ma.getElementAt(2, 0);
29156
29157 calibrator.setInitialMzx(mzx);
29158
29159
29160 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29161 }
29162
29163 @Test
29164 public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
29165 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29166 new KnownBiasAndGravityNormAccelerometerCalibrator();
29167
29168 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29169
29170
29171 final Matrix ma = generateMaGeneral();
29172 final double mzy = ma.getElementAt(2, 1);
29173
29174 calibrator.setInitialMzy(mzy);
29175
29176
29177 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29178 }
29179
29180 @Test
29181 public void testSetInitialScalingFactors()
29182 throws WrongSizeException, LockedException {
29183
29184 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29185 new KnownBiasAndGravityNormAccelerometerCalibrator();
29186
29187
29188 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29189 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29190 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29191
29192
29193 final Matrix ma = generateMaGeneral();
29194 final double sx = ma.getElementAt(0, 0);
29195 final double sy = ma.getElementAt(1, 1);
29196 final double sz = ma.getElementAt(2, 2);
29197
29198 calibrator.setInitialScalingFactors(sx, sy, sz);
29199
29200
29201 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29202 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29203 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29204 }
29205
29206 @Test
29207 public void testSetInitialCrossCouplingErrors()
29208 throws WrongSizeException, LockedException {
29209
29210 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29211 new KnownBiasAndGravityNormAccelerometerCalibrator();
29212
29213
29214 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29215 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29216 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29217 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29218 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29219 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29220
29221
29222 final Matrix ma = generateMaGeneral();
29223 final double mxy = ma.getElementAt(0, 1);
29224 final double mxz = ma.getElementAt(0, 2);
29225 final double myx = ma.getElementAt(1, 0);
29226 final double myz = ma.getElementAt(1, 2);
29227 final double mzx = ma.getElementAt(2, 0);
29228 final double mzy = ma.getElementAt(2, 1);
29229
29230 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
29231
29232
29233 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29234 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29235 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29236 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29237 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29238 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29239 }
29240
29241 @Test
29242 public void testSetInitialScalingFactorsAndCrossCouplingErrors()
29243 throws WrongSizeException, LockedException {
29244
29245 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29246 new KnownBiasAndGravityNormAccelerometerCalibrator();
29247
29248
29249 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29250 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29251 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29252 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29253 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29254 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29255 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29256 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29257 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29258
29259
29260 final Matrix ma = generateMaGeneral();
29261 final double sx = ma.getElementAt(0, 0);
29262 final double sy = ma.getElementAt(1, 1);
29263 final double sz = ma.getElementAt(2, 2);
29264 final double mxy = ma.getElementAt(0, 1);
29265 final double mxz = ma.getElementAt(0, 2);
29266 final double myx = ma.getElementAt(1, 0);
29267 final double myz = ma.getElementAt(1, 2);
29268 final double mzx = ma.getElementAt(2, 0);
29269 final double mzy = ma.getElementAt(2, 1);
29270
29271 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
29272 mxy, mxz, myx, myz, mzx, mzy);
29273
29274
29275 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29276 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29277 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29278 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29279 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29280 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29281 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29282 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29283 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29284 }
29285
29286 @Test
29287 public void testGetSetBias() throws LockedException {
29288 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29289 new KnownBiasAndGravityNormAccelerometerCalibrator();
29290
29291
29292 final double[] bias1 = calibrator.getBias();
29293 assertArrayEquals(bias1, new double[3], 0.0);
29294
29295
29296 final Matrix ba = generateBa();
29297 final double[] bias2 = ba.getBuffer();
29298 calibrator.setBias(bias2);
29299
29300
29301 final double[] bias3 = calibrator.getBias();
29302 final double[] bias4 = new double[3];
29303 calibrator.getBias(bias4);
29304
29305 assertArrayEquals(bias2, bias3, 0.0);
29306 assertArrayEquals(bias2, bias4, 0.0);
29307
29308
29309 try {
29310 calibrator.getBias(new double[1]);
29311 fail("IllegalArgumentException expected but not thrown");
29312 } catch (final IllegalArgumentException ignore) {
29313 }
29314 try {
29315 calibrator.setBias(new double[1]);
29316 fail("IllegalArgumentException expected but not thrown");
29317 } catch (final IllegalArgumentException ignore) {
29318 }
29319 }
29320
29321 @Test
29322 public void testGetSetBiasAsMatrix() throws WrongSizeException,
29323 LockedException {
29324
29325 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29326 new KnownBiasAndGravityNormAccelerometerCalibrator();
29327
29328
29329 final Matrix bias1 = calibrator.getBiasAsMatrix();
29330 assertEquals(bias1, new Matrix(3, 1));
29331
29332
29333 final Matrix bias2 = generateBa();
29334 calibrator.setBias(bias2);
29335
29336
29337 final Matrix bias3 = calibrator.getBiasAsMatrix();
29338 final Matrix bias4 = new Matrix(3, 1);
29339 calibrator.getBiasAsMatrix(bias4);
29340
29341 assertEquals(bias2, bias3);
29342 assertEquals(bias2, bias4);
29343
29344
29345 try {
29346 calibrator.getBiasAsMatrix(new Matrix(1, 1));
29347 fail("IllegalArgumentException expected but not thrown");
29348 } catch (final IllegalArgumentException ignore) {
29349 }
29350 try {
29351 calibrator.getBiasAsMatrix(new Matrix(3, 3));
29352 fail("IllegalArgumentException expected but not thrown");
29353 } catch (final IllegalArgumentException ignore) {
29354 }
29355 try {
29356 calibrator.setBias(new Matrix(1, 1));
29357 fail("IllegalArgumentException expected but not thrown");
29358 } catch (final IllegalArgumentException ignore) {
29359 }
29360 try {
29361 calibrator.setBias(new Matrix(3, 3));
29362 fail("IllegalArgumentException expected but not thrown");
29363 } catch (final IllegalArgumentException ignore) {
29364 }
29365 }
29366
29367 @Test
29368 public void testGetSetInitialMa() throws WrongSizeException, LockedException {
29369 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29370 new KnownBiasAndGravityNormAccelerometerCalibrator();
29371
29372
29373 final Matrix ma1 = calibrator.getInitialMa();
29374 assertEquals(ma1, new Matrix(3, 3));
29375
29376
29377 final Matrix ma2 = generateMaGeneral();
29378 calibrator.setInitialMa(ma2);
29379
29380
29381 final Matrix ma3 = calibrator.getInitialMa();
29382 final Matrix ma4 = new Matrix(3, 3);
29383 calibrator.getInitialMa(ma4);
29384
29385 assertEquals(ma2, ma3);
29386 assertEquals(ma2, ma4);
29387
29388
29389 try {
29390 calibrator.getInitialMa(new Matrix(1, 3));
29391 fail("IllegalArgumentException expected but not thrown");
29392 } catch (final IllegalArgumentException ignore) {
29393 }
29394 try {
29395 calibrator.getInitialMa(new Matrix(3, 1));
29396 fail("IllegalArgumentException expected but not thrown");
29397 } catch (final IllegalArgumentException ignore) {
29398 }
29399 try {
29400 calibrator.setInitialMa(new Matrix(1, 3));
29401 fail("IllegalArgumentException expected but not thrown");
29402 } catch (final IllegalArgumentException ignore) {
29403 }
29404 try {
29405 calibrator.setInitialMa(new Matrix(3, 1));
29406 fail("IllegalArgumentException expected but not thrown");
29407 } catch (final IllegalArgumentException ignore) {
29408 }
29409 }
29410
29411 @Test
29412 public void testGetSetMeasurements() throws LockedException {
29413 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29414 new KnownBiasAndGravityNormAccelerometerCalibrator();
29415
29416
29417 assertNull(calibrator.getMeasurements());
29418
29419
29420 final Collection<StandardDeviationBodyKinematics> measurements =
29421 Collections.emptyList();
29422 calibrator.setMeasurements(measurements);
29423
29424
29425 assertSame(calibrator.getMeasurements(), measurements);
29426 }
29427
29428 @Test
29429 public void testIsSetCommonAxisUsed() throws LockedException {
29430 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29431 new KnownBiasAndGravityNormAccelerometerCalibrator();
29432
29433
29434 assertFalse(calibrator.isCommonAxisUsed());
29435
29436
29437 calibrator.setCommonAxisUsed(true);
29438
29439
29440 assertTrue(calibrator.isCommonAxisUsed());
29441 }
29442
29443 @Test
29444 public void testGetSetListener() throws LockedException {
29445 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29446 new KnownBiasAndGravityNormAccelerometerCalibrator();
29447
29448
29449 assertNull(calibrator.getListener());
29450
29451
29452 calibrator.setListener(this);
29453
29454
29455 assertSame(calibrator.getListener(), this);
29456 }
29457
29458 @Test
29459 public void testGetSetGroundTruthGravityNorm1() throws LockedException {
29460 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29461 new KnownBiasAndGravityNormAccelerometerCalibrator();
29462
29463
29464 assertNull(calibrator.getGroundTruthGravityNorm());
29465
29466
29467 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29468 final double latitude = Math.toRadians(
29469 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29470 final double longitude = Math.toRadians(
29471 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29472 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29473 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29474 final NEDVelocity nedVelocity = new NEDVelocity();
29475 final ECEFPosition ecefPosition = new ECEFPosition();
29476 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29477 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29478 ecefPosition, ecefVelocity);
29479 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29480 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29481 final double gravityNorm = gravity.getNorm();
29482
29483 calibrator.setGroundTruthGravityNorm(gravityNorm);
29484
29485
29486 assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(),
29487 0.0);
29488 assertEquals(gravity.getNormAsAcceleration(),
29489 calibrator.getGroundTruthGravityNormAsAcceleration());
29490 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29491 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29492 assertEquals(gravity.getNormAsAcceleration(), g);
29493 }
29494
29495 @Test
29496 public void testGetSetGroundTruthGravityNorm2() throws LockedException {
29497 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29498 new KnownBiasAndGravityNormAccelerometerCalibrator();
29499
29500
29501 assertNull(calibrator.getGroundTruthGravityNorm());
29502
29503
29504 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29505 final double latitude = Math.toRadians(
29506 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29507 final double longitude = Math.toRadians(
29508 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29509 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29510 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29511 final NEDVelocity nedVelocity = new NEDVelocity();
29512 final ECEFPosition ecefPosition = new ECEFPosition();
29513 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29514 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29515 ecefPosition, ecefVelocity);
29516 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29517 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29518 final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29519
29520 calibrator.setGroundTruthGravityNorm(gravityNorm);
29521
29522
29523 assertEquals(gravity.getNorm(),
29524 calibrator.getGroundTruthGravityNorm(), 0.0);
29525 assertEquals(gravityNorm,
29526 calibrator.getGroundTruthGravityNormAsAcceleration());
29527 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29528 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29529 assertEquals(gravityNorm, g);
29530 }
29531
29532 @Test
29533 public void testIsReady() throws LockedException {
29534 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29535 new KnownBiasAndGravityNormAccelerometerCalibrator();
29536
29537
29538 assertFalse(calibrator.isReady());
29539
29540
29541
29542 final Collection<StandardDeviationBodyKinematics> measurements1 =
29543 Collections.emptyList();
29544 calibrator.setMeasurements(measurements1);
29545
29546
29547 assertFalse(calibrator.isReady());
29548
29549
29550
29551 calibrator.setCommonAxisUsed(false);
29552
29553 final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
29554 for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29555 measurements2.add(new StandardDeviationBodyKinematics());
29556 }
29557 calibrator.setMeasurements(measurements2);
29558
29559
29560 assertFalse(calibrator.isReady());
29561
29562
29563
29564 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29565 final double latitude = Math.toRadians(
29566 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29567 final double longitude = Math.toRadians(
29568 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29569 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29570 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29571 final NEDVelocity nedVelocity = new NEDVelocity();
29572 final ECEFPosition ecefPosition = new ECEFPosition();
29573 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29574 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29575 ecefPosition, ecefVelocity);
29576 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29577 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29578
29579 calibrator.setGroundTruthGravityNorm(gravity.getNorm());
29580
29581 assertTrue(calibrator.isReady());
29582
29583
29584 measurements2.clear();
29585 for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29586 measurements2.add(new StandardDeviationBodyKinematics());
29587 }
29588 calibrator.setMeasurements(measurements2);
29589
29590
29591 assertFalse(calibrator.isReady());
29592
29593 calibrator.setCommonAxisUsed(true);
29594
29595 assertTrue(calibrator.isReady());
29596 }
29597
29598 @Test
29599 public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
29600 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29601 LockedException, NotReadyException {
29602
29603 int numValid = 0;
29604 for (int t = 0; t < TIMES; t++) {
29605 final Matrix ba = generateBa();
29606 final Matrix bg = generateBg();
29607 final Matrix ma = generateMaGeneral();
29608 final Matrix mg = generateMg();
29609 final Matrix gg = generateGg();
29610
29611
29612 final double accelNoiseRootPSD = 0.0;
29613 final double gyroNoiseRootPSD = 0.0;
29614 final double accelQuantLevel = 0.0;
29615 final double gyroQuantLevel = 0.0;
29616
29617 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29618 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29619
29620 final Random random = new Random();
29621 final UniformRandomizer randomizer = new UniformRandomizer(random);
29622 final double latitude = Math.toRadians(
29623 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29624 final double longitude = Math.toRadians(
29625 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29626 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29627 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29628 final NEDVelocity nedVelocity = new NEDVelocity();
29629 final ECEFPosition ecefPosition = new ECEFPosition();
29630 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29631 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29632 ecefPosition, ecefVelocity);
29633 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29634 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29635
29636 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29637 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29638 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29639
29640 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29641 for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29642 final double roll = Math.toRadians(
29643 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29644 final double pitch = Math.toRadians(
29645 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29646 final double yaw = Math.toRadians(
29647 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29648 final CoordinateTransformation nedC = new CoordinateTransformation(
29649 roll, pitch, yaw, FrameType.BODY_FRAME,
29650 FrameType.LOCAL_NAVIGATION_FRAME);
29651
29652 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29653 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29654 .convertNEDtoECEFAndReturnNew(nedFrame);
29655
29656
29657
29658 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29659 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29660 ecefFrame);
29661
29662
29663
29664 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29665 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29666
29667 final StandardDeviationBodyKinematics measurement =
29668 new StandardDeviationBodyKinematics(measuredKinematics,
29669 specificForceStandardDeviation,
29670 angularRateStandardDeviation);
29671 measurements.add(measurement);
29672 }
29673
29674
29675
29676 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29677 new KnownBiasAndGravityNormAccelerometerCalibrator(
29678 gravity.getNorm(), measurements,
29679 false, ba, ma, this);
29680
29681
29682 reset();
29683 assertTrue(calibrator.isReady());
29684 assertFalse(calibrator.isRunning());
29685 assertEquals(mCalibrateStart, 0);
29686 assertEquals(mCalibrateEnd, 0);
29687
29688 try {
29689 calibrator.calibrate();
29690 } catch (final CalibrationException e) {
29691 continue;
29692 }
29693
29694
29695 assertTrue(calibrator.isReady());
29696 assertFalse(calibrator.isRunning());
29697 assertEquals(mCalibrateStart, 1);
29698 assertEquals(mCalibrateEnd, 1);
29699
29700 final Matrix estimatedMa = calibrator.getEstimatedMa();
29701
29702 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29703 continue;
29704 }
29705
29706 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29707
29708 numValid++;
29709
29710 break;
29711 }
29712
29713 assertTrue(numValid > 0);
29714 }
29715
29716 @Test
29717 public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
29718 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29719 LockedException, NotReadyException {
29720
29721 int numValid = 0;
29722 for (int t = 0; t < TIMES; t++) {
29723 final Matrix ba = generateBa();
29724 final Matrix bg = generateBg();
29725 final Matrix ma = generateMaGeneral();
29726 final Matrix mg = generateMg();
29727 final Matrix gg = generateGg();
29728 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29729 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29730 final double accelQuantLevel = 0.0;
29731 final double gyroQuantLevel = 0.0;
29732
29733 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29734 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29735
29736 final Random random = new Random();
29737 final UniformRandomizer randomizer = new UniformRandomizer(random);
29738 final double latitude = Math.toRadians(
29739 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29740 final double longitude = Math.toRadians(
29741 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29742 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29743 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29744 final NEDVelocity nedVelocity = new NEDVelocity();
29745 final ECEFPosition ecefPosition = new ECEFPosition();
29746 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29747 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29748 ecefPosition, ecefVelocity);
29749 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29750 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29751
29752 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29753 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29754 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29755
29756 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29757 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29758 final double roll = Math.toRadians(
29759 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29760 final double pitch = Math.toRadians(
29761 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29762 final double yaw = Math.toRadians(
29763 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29764 final CoordinateTransformation nedC = new CoordinateTransformation(
29765 roll, pitch, yaw, FrameType.BODY_FRAME,
29766 FrameType.LOCAL_NAVIGATION_FRAME);
29767
29768 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29769 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29770 .convertNEDtoECEFAndReturnNew(nedFrame);
29771
29772
29773
29774 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29775 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29776 ecefFrame);
29777
29778
29779
29780 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29781 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29782
29783 final StandardDeviationBodyKinematics measurement =
29784 new StandardDeviationBodyKinematics(measuredKinematics,
29785 specificForceStandardDeviation,
29786 angularRateStandardDeviation);
29787 measurements.add(measurement);
29788 }
29789
29790
29791
29792 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29793 new KnownBiasAndGravityNormAccelerometerCalibrator(
29794 gravity.getNorm(),
29795 measurements, false, this);
29796
29797
29798 reset();
29799 assertTrue(calibrator.isReady());
29800 assertFalse(calibrator.isRunning());
29801 assertEquals(mCalibrateStart, 0);
29802 assertEquals(mCalibrateEnd, 0);
29803
29804 try {
29805 calibrator.calibrate();
29806 } catch (final CalibrationException e) {
29807 continue;
29808 }
29809
29810
29811 assertTrue(calibrator.isReady());
29812 assertFalse(calibrator.isRunning());
29813 assertEquals(mCalibrateStart, 1);
29814 assertEquals(mCalibrateEnd, 1);
29815
29816 final Matrix estimatedMa = calibrator.getEstimatedMa();
29817
29818 if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
29819 continue;
29820 }
29821
29822 assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
29823
29824 numValid++;
29825
29826 break;
29827 }
29828
29829 assertTrue(numValid > 0);
29830 }
29831
29832 @Test
29833 public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
29834 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29835 LockedException, NotReadyException {
29836
29837 int numValid = 0;
29838 for (int t = 0; t < TIMES; t++) {
29839 final Matrix ba = generateBa();
29840 final Matrix bg = generateBg();
29841 final Matrix ma = generateMaCommonAxis();
29842 final Matrix mg = generateMg();
29843 final Matrix gg = generateGg();
29844
29845
29846 final double accelNoiseRootPSD = 0.0;
29847 final double gyroNoiseRootPSD = 0.0;
29848 final double accelQuantLevel = 0.0;
29849 final double gyroQuantLevel = 0.0;
29850
29851 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29852 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29853
29854 final Random random = new Random();
29855 final UniformRandomizer randomizer = new UniformRandomizer(random);
29856 final double latitude = Math.toRadians(
29857 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29858 final double longitude = Math.toRadians(
29859 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29860 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29861 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29862 final NEDVelocity nedVelocity = new NEDVelocity();
29863 final ECEFPosition ecefPosition = new ECEFPosition();
29864 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29865 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29866 ecefPosition, ecefVelocity);
29867 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29868 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29869
29870 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29871 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29872 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29873
29874 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29875 for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29876 final double roll = Math.toRadians(
29877 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29878 final double pitch = Math.toRadians(
29879 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29880 final double yaw = Math.toRadians(
29881 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29882 final CoordinateTransformation nedC = new CoordinateTransformation(
29883 roll, pitch, yaw, FrameType.BODY_FRAME,
29884 FrameType.LOCAL_NAVIGATION_FRAME);
29885
29886 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29887 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29888 .convertNEDtoECEFAndReturnNew(nedFrame);
29889
29890
29891
29892 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29893 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29894 ecefFrame);
29895
29896
29897
29898 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29899 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29900
29901 final StandardDeviationBodyKinematics measurement =
29902 new StandardDeviationBodyKinematics(measuredKinematics,
29903 specificForceStandardDeviation,
29904 angularRateStandardDeviation);
29905 measurements.add(measurement);
29906 }
29907
29908
29909
29910 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29911 new KnownBiasAndGravityNormAccelerometerCalibrator(
29912 gravity.getNorm(), measurements,
29913 true, ba, ma, this);
29914
29915
29916 reset();
29917 assertTrue(calibrator.isReady());
29918 assertFalse(calibrator.isRunning());
29919 assertEquals(mCalibrateStart, 0);
29920 assertEquals(mCalibrateEnd, 0);
29921
29922 try {
29923 calibrator.calibrate();
29924 } catch (final CalibrationException e) {
29925 continue;
29926 }
29927
29928
29929 assertTrue(calibrator.isReady());
29930 assertFalse(calibrator.isRunning());
29931 assertEquals(mCalibrateStart, 1);
29932 assertEquals(mCalibrateEnd, 1);
29933
29934 final Matrix estimatedMa = calibrator.getEstimatedMa();
29935
29936 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29937 continue;
29938 }
29939
29940 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29941
29942 numValid++;
29943
29944 break;
29945 }
29946
29947 assertTrue(numValid > 0);
29948 }
29949
29950 @Test
29951 public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
29952 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29953 LockedException, NotReadyException {
29954
29955 int numValid = 0;
29956 for (int t = 0; t < TIMES; t++) {
29957 final Matrix ba = generateBa();
29958 final Matrix bg = generateBg();
29959 final Matrix ma = generateMaCommonAxis();
29960 final Matrix mg = generateMg();
29961 final Matrix gg = generateGg();
29962 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29963 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29964 final double accelQuantLevel = 0.0;
29965 final double gyroQuantLevel = 0.0;
29966
29967 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29968 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29969
29970 final Random random = new Random();
29971 final UniformRandomizer randomizer = new UniformRandomizer(random);
29972 final double latitude = Math.toRadians(
29973 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29974 final double longitude = Math.toRadians(
29975 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29976 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29977 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29978 final NEDVelocity nedVelocity = new NEDVelocity();
29979 final ECEFPosition ecefPosition = new ECEFPosition();
29980 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29981 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29982 ecefPosition, ecefVelocity);
29983 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29984 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29985
29986 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29987 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29988 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29989
29990 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29991 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29992 final double roll = Math.toRadians(
29993 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29994 final double pitch = Math.toRadians(
29995 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29996 final double yaw = Math.toRadians(
29997 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29998 final CoordinateTransformation nedC = new CoordinateTransformation(
29999 roll, pitch, yaw, FrameType.BODY_FRAME,
30000 FrameType.LOCAL_NAVIGATION_FRAME);
30001
30002 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30003 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30004 .convertNEDtoECEFAndReturnNew(nedFrame);
30005
30006
30007
30008 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30009 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30010 ecefFrame);
30011
30012
30013
30014 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30015 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30016
30017 final StandardDeviationBodyKinematics measurement =
30018 new StandardDeviationBodyKinematics(measuredKinematics,
30019 specificForceStandardDeviation,
30020 angularRateStandardDeviation);
30021 measurements.add(measurement);
30022 }
30023
30024
30025
30026 final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
30027 new KnownBiasAndGravityNormAccelerometerCalibrator(
30028 gravity.getNorm(), measurements,
30029 true, this);
30030
30031
30032 reset();
30033 assertTrue(calibrator.isReady());
30034 assertFalse(calibrator.isRunning());
30035 assertEquals(mCalibrateStart, 0);
30036 assertEquals(mCalibrateEnd, 0);
30037
30038 try {
30039 calibrator.calibrate();
30040 } catch (final CalibrationException e) {
30041 continue;
30042 }
30043
30044
30045 assertTrue(calibrator.isReady());
30046 assertFalse(calibrator.isRunning());
30047 assertEquals(mCalibrateStart, 1);
30048 assertEquals(mCalibrateEnd, 1);
30049
30050 final Matrix estimatedMa = calibrator.getEstimatedMa();
30051
30052 if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
30053 continue;
30054 }
30055
30056 assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
30057
30058 numValid++;
30059
30060 break;
30061 }
30062
30063 assertTrue(numValid > 0);
30064 }
30065
30066 @Override
30067 public void onCalibrateStart(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30068 checkLocked(calibrator);
30069 mCalibrateStart++;
30070 }
30071
30072 @Override
30073 public void onCalibrateEnd(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30074 checkLocked(calibrator);
30075 mCalibrateEnd++;
30076 }
30077
30078 private void reset() {
30079 mCalibrateStart = 0;
30080 mCalibrateEnd = 0;
30081 }
30082
30083 private void checkLocked(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30084 assertTrue(calibrator.isRunning());
30085 try {
30086 calibrator.setBiasX(0.0);
30087 fail("LockedException expected but not thrown");
30088 } catch (final LockedException ignore) {
30089 }
30090 try {
30091 calibrator.setBiasY(0.0);
30092 fail("LockedException expected but not thrown");
30093 } catch (final LockedException ignore) {
30094 }
30095 try {
30096 calibrator.setBiasZ(0.0);
30097 fail("LockedException expected but not thrown");
30098 } catch (final LockedException ignore) {
30099 }
30100 try {
30101 calibrator.setBiasX(null);
30102 fail("LockedException expected but not thrown");
30103 } catch (final LockedException ignore) {
30104 }
30105 try {
30106 calibrator.setBiasY(null);
30107 fail("LockedException expected but not thrown");
30108 } catch (final LockedException ignore) {
30109 }
30110 try {
30111 calibrator.setBiasZ(null);
30112 fail("LockedException expected but not thrown");
30113 } catch (final LockedException ignore) {
30114 }
30115 try {
30116 calibrator.setBias(0.0, 0.0, 0.0);
30117 fail("LockedException expected but not thrown");
30118 } catch (final LockedException ignore) {
30119 }
30120 try {
30121 calibrator.setBias(null, null, null);
30122 fail("LockedException expected but not thrown");
30123 } catch (final LockedException ignore) {
30124 }
30125 try {
30126 calibrator.setInitialSx(0.0);
30127 fail("LockedException expected but not thrown");
30128 } catch (final LockedException ignore) {
30129 }
30130 try {
30131 calibrator.setInitialSy(0.0);
30132 fail("LockedException expected but not thrown");
30133 } catch (final LockedException ignore) {
30134 }
30135 try {
30136 calibrator.setInitialSz(0.0);
30137 fail("LockedException expected but not thrown");
30138 } catch (final LockedException ignore) {
30139 }
30140 try {
30141 calibrator.setInitialMxy(0.0);
30142 fail("LockedException expected but not thrown");
30143 } catch (final LockedException ignore) {
30144 }
30145 try {
30146 calibrator.setInitialMxz(0.0);
30147 fail("LockedException expected but not thrown");
30148 } catch (final LockedException ignore) {
30149 }
30150 try {
30151 calibrator.setInitialMyx(0.0);
30152 fail("LockedException expected but not thrown");
30153 } catch (final LockedException ignore) {
30154 }
30155 try {
30156 calibrator.setInitialMyz(0.0);
30157 fail("LockedException expected but not thrown");
30158 } catch (final LockedException ignore) {
30159 }
30160 try {
30161 calibrator.setInitialMzx(0.0);
30162 fail("LockedException expected but not thrown");
30163 } catch (final LockedException ignore) {
30164 }
30165 try {
30166 calibrator.setInitialMzy(0.0);
30167 fail("LockedException expected but not thrown");
30168 } catch (final LockedException ignore) {
30169 }
30170 try {
30171 calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
30172 fail("LockedException expected but not thrown");
30173 } catch (final LockedException ignore) {
30174 }
30175 try {
30176 calibrator.setInitialCrossCouplingErrors(
30177 0.0, 0.0, 0.0,
30178 0.0, 0.0, 0.0);
30179 fail("LockedException expected but not thrown");
30180 } catch (final LockedException ignore) {
30181 }
30182 try {
30183 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
30184 0.0, 0.0, 0.0,
30185 0.0, 0.0, 0.0,
30186 0.0, 0.0, 0.0);
30187 fail("LockedException expected but not thrown");
30188 } catch (final LockedException ignore) {
30189 }
30190 try {
30191 calibrator.setBias((double[]) null);
30192 fail("LockedException expected but not thrown");
30193 } catch (final LockedException ignore) {
30194 }
30195 try {
30196 calibrator.setBias((Matrix) null);
30197 fail("LockedException expected but not thrown");
30198 } catch (final LockedException ignore) {
30199 }
30200 try {
30201 calibrator.setInitialMa(null);
30202 fail("LockedException expected but not thrown");
30203 } catch (final LockedException ignore) {
30204 }
30205 try {
30206 calibrator.setGroundTruthGravityNorm(0.0);
30207 fail("LockedException expected but not thrown");
30208 } catch (final LockedException ignore) {
30209 }
30210 try {
30211 calibrator.setGroundTruthGravityNorm((Acceleration) null);
30212 fail("LockedException expected but not thrown");
30213 } catch (final LockedException ignore) {
30214 }
30215 try {
30216 calibrator.setMeasurements(null);
30217 fail("LockedException expected but not thrown");
30218 } catch (final LockedException ignore) {
30219 }
30220 try {
30221 calibrator.setCommonAxisUsed(true);
30222 fail("LockedException expected but not thrown");
30223 } catch (final LockedException ignore) {
30224 }
30225 try {
30226 calibrator.setListener(this);
30227 fail("LockedException expected but not thrown");
30228 } catch (final LockedException ignore) {
30229 }
30230 try {
30231 calibrator.calibrate();
30232 fail("LockedException expected but not thrown");
30233 } catch (final LockedException ignore) {
30234 } catch (final Exception e) {
30235 fail("LockedException expected but not thrown");
30236 }
30237 }
30238
30239 private Matrix generateBa() {
30240 return Matrix.newFromArray(new double[]{
30241 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30242 -1000 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30243 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
30244 }
30245
30246 private Matrix generateBg() {
30247 return Matrix.newFromArray(new double[]{
30248 -9 * DEG_TO_RAD / 3600.0,
30249 10 * DEG_TO_RAD / 3600.0,
30250 -8 * DEG_TO_RAD / 3600.0});
30251 }
30252
30253 private Matrix generateMaGeneral() throws WrongSizeException {
30254 final Matrix result = new Matrix(3, 3);
30255 result.fromArray(new double[]{
30256 500e-6, -300e-6, 200e-6,
30257 -150e-6, -600e-6, 250e-6,
30258 -250e-6, 70e-6, 450e-6
30259 }, false);
30260
30261 return result;
30262 }
30263
30264 private Matrix generateMaCommonAxis() throws WrongSizeException {
30265 final Matrix result = new Matrix(3, 3);
30266 result.fromArray(new double[]{
30267 500e-6, -300e-6, 200e-6,
30268 0.0, -600e-6, 250e-6,
30269 0.0, 0.0, 450e-6
30270 }, false);
30271
30272 return result;
30273 }
30274
30275 private Matrix generateMg() throws WrongSizeException {
30276 final Matrix result = new Matrix(3, 3);
30277 result.fromArray(new double[]{
30278 400e-6, -300e-6, 250e-6,
30279 0.0, -300e-6, -150e-6,
30280 0.0, 0.0, -350e-6
30281 }, false);
30282
30283 return result;
30284 }
30285
30286 private Matrix generateGg() throws WrongSizeException {
30287 final Matrix result = new Matrix(3, 3);
30288 final double tmp = DEG_TO_RAD / (3600 * 9.80665);
30289 result.fromArray(new double[]{
30290 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
30291 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
30292 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
30293 }, false);
30294
30295 return result;
30296 }
30297
30298 private double getAccelNoiseRootPSD() {
30299 return 70.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
30300 }
30301
30302 private double getGyroNoiseRootPSD() {
30303 return 0.01 * DEG_TO_RAD / 60.0;
30304 }
30305 }